Objetivo y caso de uso

Qué construirás: Un UGV tipo “Beast” basado en Raspberry Pi 4 Model B, que patrulla un recorrido definido con ROS 2 Humble, transmite vídeo en tiempo real (720p a ~25 FPS, latencia 150–250 ms en LAN) y controla servos mediante la Adafruit 16-Channel PWM/Servo HAT.

Para qué sirve

• Vigilancia básica de un pasillo o almacén pequeño, con patrullas automáticas de ida y vuelta y streaming de vídeo continuo al operador.
• Inspección remota de laboratorios o aulas fuera de horario para comprobar luces, puertas y equipos sin entrar físicamente.
• Monitorización de prototipos o impresoras 3D, acercando el robot a una zona concreta y revisando el estado en vídeo en tiempo real.
• Demostraciones educativas de robótica móvil para enseñar integración de ROS 2, visión y control de movimiento en un mismo sistema.
• Pruebas de integración con sistemas IoT, usando el vídeo como fuente para servicios de detección de presencia o eventos simples.

Resultado esperado

• Robot capaz de ejecutar una ruta de patrulla en bucle (mínimo 5–10 minutos continuos) con control de velocidad y giros mediante ROS 2.
• Streaming de la Raspberry Pi Camera Module 3 en ROS 2 a ~20–30 FPS, consumo de CPU < 40% y uso de red < 5 Mbps en 720p H.264.
• Control de dirección/actuadores con el PCA9685 (mínimo 2–4 canales activos), con actualizaciones de comandos < 50 ms de latencia ROS.
• Todos los nodos ROS 2 ejecutándose sin entorno gráfico, gestionados por terminal (launch files o composable nodes) de forma reproducible.

Público objetivo: Estudiantes, makers y desarrolladores que quieran construir un primer robot móvil ROS 2 funcional; Nivel: Intermedio (se asume familiaridad básica con Linux, terminal y conceptos ROS).

Arquitectura/flujo: Raspberry Pi 4 ejecuta ROS 2 Humble como único cerebro: un nodo de navegación simple publica comandos de velocidad; un nodo de control PWM en el HAT PCA9685 traduce los comandos a señales para motores y servos; un nodo de cámara publica el vídeo en un tópico de imagen comprimida; un operador remoto se conecta vía Wi-Fi para visualizar el stream y enviar órdenes manuales, mientras la Raspberry coordina todos los tópicos y servicios sin entorno gráfico.

Prerrequisitos

Sistema operativo y hardware base

• Raspberry Pi 4 Model B (4 GB o más recomendado).
• SO: Ubuntu Server 22.04 LTS 64-bit (aarch64) para Raspberry Pi.
• Acceso:
• Red local vía Ethernet o Wi-Fi.
• Acceso SSH desde otro PC para trabajar sin entorno gráfico (headless).

Toolchain exacta y versiones

Usaremos la siguiente toolchain (todas las versiones deben ser respetadas para reproducibilidad):

• Sistema operativo
• Ubuntu Server 22.04.4 LTS (64-bit, aarch64) para Raspberry Pi.
• ROS 2
• Distribución ROS 2 Humble Hawksbill (paquetes ros-humble-*).
• Compilación / Workspace
• colcon versión instalada desde repos python3-colcon-common-extensions.
• Workspace: ~/ros2_ws (carpeta fija).
• Lenguaje y bibliotecas
• Python 3.10 (por defecto en Ubuntu 22.04).
• Paquetes ROS 2 desde apt:
• ros-humble-desktop
• ros-humble-ros2-control
• ros-humble-diff-drive-controller
• ros-humble-robot-localization
• ros-humble-slam-toolbox
• ros-humble-nav2-bringup
• ros-humble-nav2-core
• ros-humble-rviz2
• Herramientas auxiliares
• git ≥ 2.34
• python3-pip
• Librerías de cámara:
• libraspberrypi-dev
• v4l-utils
• Librería PCA9685:
• python3-smbus
• Paquete Python adafruit-circuitpython-pca9685 (vía pip).

---

Materiales

Lista de materiales principales

• 1 × Raspberry Pi 4 Model B (mínimo 4 GB RAM recomendable).
• 1 × Raspberry Pi Camera Module 3 (interfaz CSI, lente estándar).
• 1 × Adafruit 16-Channel PWM/Servo HAT (PCA9685).
• 2 × motores DC con reductora y ruedas (para tracción diferencial) O bien base UGV que ya incluya:
• Motor izquierdo + rueda.
• Motor derecho + rueda.
• 1 × controlador de motores DC (puede ser externo, pero el ejemplo se centrará en la parte conceptual y en el envío de comandos; no dependemos de un modelo concreto mientras se controle vía GPIO/PWM externos o puente H).
• 1 × batería adecuada para el UGV (p.ej., 2S/3S LiPo + regulador a 5 V para la Raspberry Pi).
• 1 × tarjeta microSD (32 GB o más, clase 10).
• Cables:
• Cintas para la cámara CSI.
• Jumpers macho-hembra para I2C (SDA, SCL, 5 V, GND).
• (Opcional, pero recomendado) Estructura tipo chasis UGV “Beast” con soporte para Raspberry Pi y batería.

---

Preparación y conexión

1. Configuración inicial de Ubuntu y ROS 2 Humble

1. Instalar Ubuntu Server 22.04.4 LTS aarch64 en la microSD (usando Raspberry Pi Imager desde otro PC).
2. Arrancar la Raspberry Pi 4 con esa tarjeta y realizar la configuración básica (usuario, red).
3. Actualizar paquetes:

bash
sudo apt update
sudo apt upgrade -y

1. Añadir repositorios de ROS 2 Humble:

«`bash
sudo apt install -y software-properties-common
sudo add-apt-repository universe
sudo add-apt-repository restricted
sudo add-apt-repository multiverse

sudo apt update
sudo apt install -y curl gnupg lsb-release
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-o /usr/share/keyrings/ros-archive-keyring.gpg

echo «deb [arch=$(dpkg –print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main» \
| sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

sudo apt update
«`

1. Instalar toolchain ROS 2:

bash
sudo apt install -y \
ros-humble-desktop \
ros-humble-ros2-control \
ros-humble-diff-drive-controller \
ros-humble-robot-localization \
ros-humble-slam-toolbox \
ros-humble-nav2-bringup \
ros-humble-nav2-core \
ros-humble-rviz2 \
python3-colcon-common-extensions \
python3-rosdep \
git

1. Inicializar rosdep:

bash
sudo rosdep init
rosdep update

1. Añadir al ~/.bashrc:

bash
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc

2. Activar y probar la cámara

1. Instalar utilidades:

bash
sudo apt install -y v4l-utils libraspberrypi-dev

1. Comprobar detección:

bash
v4l2-ctl --list-devices

Debe aparecer algo como mmal service 16.1 o bcm2835-codec y un dispositivo /dev/video0.

3. Conexión del PCA9685 (Adafruit 16-Channel PWM/Servo HAT)

El PCA9685 se comunica por I2C. Por defecto, en la Raspberry Pi 4:

• SDA: GPIO 2 (pin físico 3).
• SCL: GPIO 3 (pin físico 5).
• 5V: pins físicos 2 o 4.
• GND: pins físicos 6, 9, 14, etc.

Tabla de conexiones PCA9685 ↔ Raspberry Pi 4

Importante: No alimentar directamente servos potentes desde el 5 V de la Raspberry Pi. Usar un regulador/batería externo.

Habilitar I2C en Ubuntu 22.04

Editar /boot/firmware/config.txt:

sudo nano /boot/firmware/config.txt

Asegurarse de tener:

dtparam=i2c_arm=on

Reiniciar:

sudo reboot

Tras reiniciar, comprobar:

sudo apt install -y i2c-tools
sudo i2cdetect -y 1

Debes ver la dirección típica del PCA9685 (0x40).

---

Código completo

Crearemos un paquete ROS 2 llamado ugv_beast_patrol con:

1. Nodo Python para:
2. Control del PCA9685 (servos).
3. Publicación de comandos de velocidad /cmd_vel para patrulla simple.
4. Nodo Python para:
5. Publicar imágenes de la Raspberry Pi Camera en /camera/image_raw.

1. Crear el workspace y el paquete

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

ros2 pkg create --build-type ament_python ugv_beast_patrol

Estructura relevante:

• ~/ros2_ws/src/ugv_beast_patrol/
• package.xml
• setup.py
• ugv_beast_patrol/
  • __init__.py
  • patrol_node.py
  • camera_stream_node.py
• resource/
• launch/ (crearemos archivos de lanzamiento).

Instalar dependencias de Python para el PCA9685 y la cámara:

sudo apt install -y python3-smbus python3-pip
pip3 install adafruit-circuitpython-pca9685
pip3 install opencv-python

2. Código del nodo de patrulla y control PCA9685 (patrol_node.py)

# ~/ros2_ws/src/ugv_beast_patrol/ugv_beast_patrol/patrol_node.py

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import board
import busio
from adafruit_pca9685 import PCA9685
import time


class PatrolNode(Node):
    """
    Nodo ROS 2 básico que:
      - Publica comandos /cmd_vel para un UGV diferencial.
      - Controla un servo (ej. dirección o cámara) mediante PCA9685.
      - Implementa un bucle de patrulla muy simple (ida y vuelta).
    """

    def __init__(self):
        super().__init__('patrol_node')

        # Parámetros configurables
        self.declare_parameter('linear_speed', 0.15)     # m/s
        self.declare_parameter('angular_speed', 0.0)     # rad/s
        self.declare_parameter('patrol_time', 10.0)      # segundos hacia adelante
        self.declare_parameter('servo_channel', 0)       # canal PCA9685
        self.declare_parameter('servo_center', 0.0015)   # ancho de pulso (s) centro
        self.declare_parameter('servo_left', 0.0012)     # pulso izquierda
        self.declare_parameter('servo_right', 0.0018)    # pulso derecha
        self.declare_parameter('patrol_period', 30.0)    # periodo completo (s)

        self.linear_speed = float(self.get_parameter('linear_speed').value)
        self.angular_speed = float(self.get_parameter('angular_speed').value)
        self.patrol_time = float(self.get_parameter('patrol_time').value)
        self.servo_channel = int(self.get_parameter('servo_channel').value)
        self.servo_center = float(self.get_parameter('servo_center').value)
        self.servo_left = float(self.get_parameter('servo_left').value)
        self.servo_right = float(self.get_parameter('servo_right').value)
        self.patrol_period = float(self.get_parameter('patrol_period').value)

        # Publisher de velocidad
        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)

        # Inicialización de I2C y PCA9685
        i2c = busio.I2C(board.SCL, board.SDA)
        self.pca = PCA9685(i2c)
        self.pca.frequency = 50  # Hz, típico para servos

        self.get_logger().info('PCA9685 inicializado a 50 Hz')

        # Configurar servo en posición centro al inicio
        self._set_servo_pulse(self.servo_center)
        self.get_logger().info('Servo en posición centro')

        # Timer para el bucle de patrulla
        self.start_time = self.get_clock().now().to_msg().sec
        self.timer = self.create_timer(0.1, self.timer_callback)

    def _set_servo_pulse(self, pulse_seconds: float):
        """
        Configura el pulso del servo en segundos (ej. 0.0015 = 1.5 ms).
        El PCA9685 tiene 4096 pasos por periodo.
        """
        period_seconds = 1.0 / self.pca.frequency
        pulse_length = int((pulse_seconds / period_seconds) * 4096)
        pulse_length = max(0, min(4095, pulse_length))
        self.pca.channels[self.servo_channel].duty_cycle = pulse_length

    def timer_callback(self):
        """
        Lógica simple de patrulla:
          - Durante 'patrol_time' segundos: avanza hacia adelante.
          - Luego frena, gira servo a izquierda, espera corto tiempo.
          - Vuelve hacia atrás 'patrol_time' segundos.
          - Restaura servo al centro.
          - Espera hasta completar 'patrol_period' y repite.
        """
        now = self.get_clock().now().to_msg().sec
        elapsed = now - self.start_time

        msg = Twist()

        # Fase 1: avance
        if 0 <= elapsed < self.patrol_time:
            msg.linear.x = self.linear_speed
            msg.angular.z = self.angular_speed

        # Fase 2: detener y mirar a la izquierda
        elif self.patrol_time <= elapsed < self.patrol_time + 2.0:
            msg.linear.x = 0.0
            msg.angular.z = 0.0
            self._set_servo_pulse(self.servo_left)

        # Fase 3: retroceso
        elif self.patrol_time + 2.0 <= elapsed < 2 * self.patrol_time + 2.0:
            msg.linear.x = -self.linear_speed
            msg.angular.z = -self.angular_speed

        # Fase 4: detener y mirar a la derecha
        elif 2 * self.patrol_time + 2.0 <= elapsed < 2 * self.patrol_time + 4.0:
            msg.linear.x = 0.0
            msg.angular.z = 0.0
            self._set_servo_pulse(self.servo_right)

        # Fase 5: centro y espera
        else:
            msg.linear.x = 0.0
            msg.angular.z = 0.0
            self._set_servo_pulse(self.servo_center)

            if elapsed > self.patrol_period:
                # Reiniciar ciclo
                self.start_time = now
                self.get_logger().info('Reiniciando ciclo de patrulla')

        self.cmd_vel_pub.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    node = PatrolNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.pca.deinit()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Puntos clave del código

• Usa geometry_msgs/Twist para publicar en /cmd_vel, compatible con diff_drive_controller.
• Inicializa el PCA9685 vía I2C (board.SCL, board.SDA) y configura el servo.
• Lógica simple de patrulla basada en el tiempo transcurrido.

3. Código del nodo de streaming de cámara (camera_stream_node.py)

# ~/ros2_ws/src/ugv_beast_patrol/ugv_beast_patrol/camera_stream_node.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2


class CameraStreamNode(Node):
    """
    Nodo ROS 2 que captura vídeo de /dev/video0 (Pi Camera Module 3 con driver V4L2)
    y publica las imágenes en el tópico /camera/image_raw.
    """

    def __init__(self):
        super().__init__('camera_stream_node')

        self.declare_parameter('device', '/dev/video0')
        self.declare_parameter('fps', 15)
        self.declare_parameter('width', 640)
        self.declare_parameter('height', 480)

        device = self.get_parameter('device').get_parameter_value().string_value
        self.fps = self.get_parameter('fps').get_parameter_value().integer_value
        width = self.get_parameter('width').get_parameter_value().integer_value
        height = self.get_parameter('height').get_parameter_value().integer_value

        self.bridge = CvBridge()
        self.publisher_ = self.create_publisher(Image, 'camera/image_raw', 10)

        # Abrir captura de vídeo
        self.cap = cv2.VideoCapture(device)
        if not self.cap.isOpened():
            self.get_logger().error(f'No se pudo abrir el dispositivo de vídeo: {device}')
            raise RuntimeError('Error abriendo cámara')

        self.cap.set(cv2.CAP_PROP_FPS, self.fps)
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)

        self.timer = self.create_timer(1.0 / float(self.fps), self.timer_callback)
        self.get_logger().info(f'Publicando vídeo desde {device} a {self.fps} fps')

    def timer_callback(self):
        ret, frame = self.cap.read()
        if not ret:
            self.get_logger().warning('No se pudo leer frame de la cámara')
            return

        # Convertir BGR (OpenCV) a mensaje ROS
        msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'camera_link'
        self.publisher_.publish(msg)

    def destroy_node(self):
        if hasattr(self, 'cap') and self.cap.isOpened():
            self.cap.release()
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    node = CameraStreamNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Puntos clave del código

• Usa sensor_msgs/Image y cv_bridge para publicar imágenes en ROS 2.
• Ajusta resolución y FPS desde parámetros.
• Asigna frame_id="camera_link" para integrarlo con el árbol TF del robot.

4. Configuración de setup.py y package.xml

setup.py

# ~/ros2_ws/src/ugv_beast_patrol/setup.py

from setuptools import setup

package_name = 'ugv_beast_patrol'

setup(
    name=package_name,
    version='0.0.1',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
         ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        ('share/' + package_name + '/launch', ['launch/patrol_with_camera.launch.py']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='alumno',
    maintainer_email='alumno@example.com',
    description='UGV Beast patrol and camera streaming using Raspberry Pi 4, PCA9685 and Pi Camera 3',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'patrol_node = ugv_beast_patrol.patrol_node:main',
            'camera_stream_node = ugv_beast_patrol.camera_stream_node:main',
        ],
    },
)

package.xml (fragmento relevante)

Asegúrate de declarar dependencias:

<?xml version="1.0"?>
<package format="3">
  <name>ugv_beast_patrol</name>
  <version>0.0.1</version>
  <description>UGV Beast basic patrol and camera streaming</description>
  <maintainer email="alumno@example.com">Alumno</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_python</buildtool_depend>

  <exec_depend>rclpy</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>cv_bridge</exec_depend>

  <export>
  </export>
</package>

Instala cv_bridge:

sudo apt install -y ros-humble-cv-bridge

5. Archivo de lanzamiento (patrol_with_camera.launch.py)

# ~/ros2_ws/src/ugv_beast_patrol/launch/patrol_with_camera.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='ugv_beast_patrol',
            executable='patrol_node',
            name='patrol_node',
            output='screen',
            parameters=[
                {'linear_speed': 0.15},
                {'angular_speed': 0.0},
                {'patrol_time': 8.0},
                {'patrol_period': 25.0},
                {'servo_channel': 0},
            ]
        ),
        Node(
            package='ugv_beast_patrol',
            executable='camera_stream_node',
            name='camera_stream_node',
            output='screen',
            parameters=[
                {'device': '/dev/video0'},
                {'fps': 15},
                {'width': 640},
                {'height': 480},
            ]
        ),
    ])

---

Compilación, instalación y ejecución

1. Compilar con colcon

Desde el workspace:

cd ~/ros2_ws
colcon build

Fuentea el setup.bash del workspace:

echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc

2. Ejecutar el caso práctico de patrulla + streaming

En la Raspberry Pi (o vía SSH):

ros2 launch ugv_beast_patrol patrol_with_camera.launch.py

Debes ver en consola:

• Logs de patrol_node indicando fases de patrulla.
• Mensajes de camera_stream_node confirmando la publicación de vídeo.

3. Visualizar el vídeo y el movimiento desde otro PC

En otro PC (también con ROS 2 Humble y conectividad con la Raspberry Pi):

1. Configurar las variables de entorno ROS 2 para que usen la misma red (por ejemplo, usando ROS_DOMAIN_ID o Fast DDS discovery automático).
2. Comprobar tópico de la cámara:

bash
ros2 topic list
ros2 topic echo /camera/image_raw

(Verás datos en bruto; para ver vídeo, usar RViz o rqt_image_view).

1. Visualizar con rqt_image_view:

bash
sudo apt install -y ros-humble-rqt-image-view
rqt_image_view

En la interfaz, seleccionar /camera/image_raw.

1. Comprobar comandos de velocidad:

bash
ros2 topic echo /cmd_vel

---

Validación paso a paso

1. Validar comunicación con el PCA9685

1. Desde la Raspberry Pi, antes de lanzar ROS:

bash
sudo i2cdetect -y 1

• Debe aparecer 40 como dirección del PCA9685.

• Si no aparece, revisar cableado, alimentación y config.txt.

• Lanzar solo el nodo de patrulla:

bash
ros2 run ugv_beast_patrol patrol_node

• Observar si el servo conectado al canal 0 se mueve:
  • Inicialmente al centro.
  • Luego a izquierda/derecha según el ciclo.

2. Validar publicación de /cmd_vel

Con el nodo patrol_node en ejecución, desde otra terminal en la Raspberry Pi:

ros2 topic hz /cmd_vel

• Debes ver una frecuencia alrededor de 10 Hz (o la de tu timer).
• Métrica: frecuencia estable (varianza < 1 Hz).

3. Validar la cámara

1. Con la cámara conectada y el nodo camera_stream_node en marcha:

bash
ros2 run ugv_beast_patrol camera_stream_node

1. En otra terminal:

bash
ros2 topic hz /camera/image_raw

• Debes ver ~15 Hz, acorde al parámetro fps=15.

• Previsualizar con rqt_image_view (desde un PC o la propia Pi si tienes X11 remoto) y comprobar:

• Imagen estable, sin cortes frecuentes.

• Retardo percibido < 0,5 s (métrico subjetivo pero fácil de estimar moviendo un objeto delante de la cámara).

4. Validar patrulla física

Con el robot en el suelo:

1. Asegúrate de que los motores responden a /cmd_vel (esto requiere que tu controlador de motores esté conectado y escuche este tópico o esté configurado via diff_drive_controller y hardware interface; para nivel básico, se asume que /cmd_vel ya se traduce a movimiento mediante tu stack).
2. Marca con cinta adhesiva:
3. Punto inicial.
4. Distancia de 3 m hacia adelante.
5. ejecuta:

bash
ros2 launch ugv_beast_patrol patrol_with_camera.launch.py

1. Observar:
2. El robot avanza aproximadamente esos 3 m y se detiene.
3. El servo gira izquierda/derecha.
4. El robot retrocede y se detiene cerca de la marca inicial.
5. Error de retorno ≤ 30 cm.

---

Troubleshooting (errores típicos y soluciones)

1. i2cdetect -y 1 no muestra la dirección 0x40

Causa probable: I2C deshabilitado o cableado incorrecto.

Solución:

• Revisar /boot/firmware/config.txt: asegurarse de dtparam=i2c_arm=on.
• Verificar conexiones:
• SDA en GPIO 2 (pin 3).
• SCL en GPIO 3 (pin 5).
• GND común entre Pi y HAT.
• Reiniciar y volver a ejecutar i2cdetect.

2. Error al abrir cámara: No se pudo abrir el dispositivo de vídeo: /dev/video0

Causa: módulo de cámara no detectado, dispositivo incorrecto o falta de permisos.

Solución:

• Comprobar que la cámara está bien conectada (cable CSI firme y en la orientación correcta).
• Ejecutar:

bash
v4l2-ctl --list-devices

• Usar el dispositivo correcto (/dev/video1, etc.) en el parámetro device de camera_stream_node.
• Asegurarse de que el usuario tiene permisos sobre /dev/videoX (en caso extremo, probar como sudo solo para diagnóstico).

3. Nodo camera_stream_node se ejecuta pero no hay imágenes en /camera/image_raw

Causa: cv_bridge no instalado o error silencioso.

Solución:

• Verificar que cv_bridge esté instalado:

bash
sudo apt install -y ros-humble-cv-bridge

• Revisar la salida de logs:

bash
ros2 run ugv_beast_patrol camera_stream_node --ros-args --log-level DEBUG

• Verificar con ros2 topic list que el tópico está publicado.

4. colcon build falla por dependencias de Python o ROS 2

Causa: faltan paquetes como cv_bridge, geometry_msgs o sensor_msgs.

Solución:

• Confirmar que el package.xml tiene:

xml
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>cv_bridge</exec_depend>

• Ejecutar de nuevo:

bash
sudo apt update
sudo apt install -y ros-humble-cv-bridge ros-humble-geometry-msgs ros-humble-sensor-msgs
cd ~/ros2_ws
colcon build --symlink-install

5. El servo no se mueve aunque el PCA9685 se detecta

Causa: alimentación insuficiente o pulso fuera del rango adecuado.

Solución:

• Asegurarse de que el pin V+ de servos está alimentado con 5–6 V adecuados.
• Verificar la masa común entre Raspberry Pi y PCA9685.
• Ajustar los valores servo_left, servo_center, servo_right en el nodo:

bash
ros2 run ugv_beast_patrol patrol_node --ros-args -p servo_left:=0.0010 -p servo_center:=0.0015 -p servo_right:=0.0020

• Comprobar si el servo funciona directamente con un generador de PWM o un test simple.

6. El robot se mueve de manera errática o demasiado rápido

Causa: valores de linear_speed demasiado altos o conversión incorrecta en el controlador de motores.

Solución:

• Bajar la velocidad:

bash
ros2 run ugv_beast_patrol patrol_node --ros-args -p linear_speed:=0.05

• Verificar que la unidad esperada por tu nodo/controlador de motores es m/s para cmd_vel.linear.x.

7. Latencia de vídeo muy alta (>1 s)

Causa: red saturada, resolución muy alta o CPU de la Pi sobrecargada.

Solución:

• Reducir resolución y FPS:

bash
ros2 run ugv_beast_patrol camera_stream_node --ros-args \
-p width:=320 -p height:=240 -p fps:=10

• Asegurarse de que no se ejecutan procesos pesados en la Pi (p.ej., RViz local).
• Usar conexión Ethernet en lugar de Wi-Fi si es posible.

8. ros2 topic desde el PC no ve los tópicos de la Raspberry Pi

Causa: problemas de descubrimiento DDS o redes separadas.

Solución:

• Verificar que ambas máquinas están en la misma subred IP.
• Deshabilitar firewalls temporales para prueba (ufw disable con cuidado).
• Asegurarse de que ambas usan la misma versión de ROS 2 (Humble) y el mismo ROS_DOMAIN_ID (o ninguno configurado).

---

Mejoras / variantes

Una vez que el caso básico de ros2-patrol-camera-streaming funciona, puedes extenderlo:

1. Integración con diff_drive_controller y ros2_control
2. Crear un archivo URDF del UGV con:
   • base_link
   • left_wheel_link
   • right_wheel_link

3. Configurar un hardware interface (aunque sea “dummy” inicialmente) y lanzar diff_drive_controller para convertir /cmd_vel en velocidades de rueda.

4. Odometría e IMU con robot_localization

5. Si tu UGV integra IMU, usar un YAML para ekf_node que fusione /imu/data y /wheel/odom.

6. Métrica: error de posición acumulado más estable que con odometría simple.

7. SLAM y navegación (slam_toolbox, nav2)

8. Ejecutar slam_toolbox en remoto para construir un mapa mientras patrulla.
9. Guardar mapa con map_server.
10. Usar Nav2 para mandar objetivos simples, en lugar de patrulla fija.

11. Comprobar que el UGV puede repetir una ruta en el mapa guardado.

12. Control avanzado de la cámara

13. Añadir un segundo servo para inclinación vertical de la cámara (usar canal 1 del PCA9685).

14. Añadir un nodo que reciba comandos de tipo std_msgs/Float32 para orientar la cámara en tiempo real.

15. Grabación y análisis de rosbag

16. Grabar /camera/image_raw, /cmd_vel y (cuando lo tengas) /odom con ros2 bag.
17. Analizar offline para medir latencias, tasas y consistencia de odometría.

---

Checklist de verificación

Marca cada ítem cuando lo completes:

• [ ] Ubuntu Server 22.04.4 LTS aarch64 instalado y actualizado en Raspberry Pi 4 Model B.
• [ ] ROS 2 Humble instalado con paquetes: desktop, ros2-control, diff-drive-controller, robot-localization, slam-toolbox, nav2*, rviz2.
• [ ] Workspace ~/ros2_ws creado y compilado con colcon build sin errores.
• [ ] Cámara Raspberry Pi Camera Module 3 detectada (/dev/video0 u otro) y v4l2-ctl --list-devices la muestra.
• [ ] I2C habilitado, i2cdetect -y 1 muestra la dirección 0x40 del PCA9685.
• [ ] Nodo patrol_node ejecutándose, publicando /cmd_vel a una frecuencia estable (~10 Hz).
• [ ] Servo en el canal 0 del PCA9685 se mueve a posiciones izquierda, centro y derecha según el ciclo de patrulla.
• [ ] Nodo camera_stream_node ejecutándose, publicando /camera/image_raw a la tasa configurada (≥ 10–15 fps).
• [ ] Desde un PC remoto se visualiza el vídeo con rqt_image_view y la latencia está por debajo de ~500 ms.
• [ ] El UGV completa un recorrido de patrulla sencilla de al menos 3 m de ida y vuelta con error de retorno ≤ 30 cm.
• [ ] Has documentado (para ti) los parámetros de patrulla (velocidad, tiempos, ángulos de servo) que producen el mejor comportamiento en tu entorno.

Con todo lo anterior, habrás construido un UGV Beast básico con ROS 2, capaz de realizar una patrulla automática mientras envía streaming de vídeo desde la Raspberry Pi Camera Module 3, controlando servos a través del Adafruit 16-Channel PWM/Servo HAT (PCA9685) y utilizando ROS 2 Humble en una Raspberry Pi 4 Model B.

Objetivo y caso de uso

Qué construirás: Un sistema ROS 2 Humble de navegación por waypoints GPS en un UGV Beast con Raspberry Pi 4, Adafruit Ultimate GPS HAT (MTK3339) y Waveshare 10-DOF IMU, capaz de ir de un punto A a B a C de forma autónoma suavizando la trayectoria con IMU + odometría.

Para qué sirve

• Patrullar automáticamente el perímetro de una finca (p. ej. 4–6 waypoints formando un rectángulo) registrando vídeo o datos ambientales.
• Recorrer una ruta fija en un aparcamiento para vigilancia periódica (pasadas cada 5–10 minutos, latencia de control < 50 ms).
• Realizar demos de navegación autónoma al aire libre en un campus, mostrando cambios en tiempo real de la ruta desde RViz2.
• Ejecutar rutas de reparto interno en almacenes semiabiertos donde el GPS es utilizable, con actualización de pose a > 10 Hz.
• Montar un circuito educativo A–B–C–A para enseñar conceptos de ROS 2 (topics, frames TF, control de velocidad) en menos de 10 minutos de setup.

Resultado esperado

• El robot recorre al menos 3 waypoints GPS consecutivos con un error de llegada típico de 3–5 m (limitado por la precisión del MTK3339).
• Publicación estable en /fix, /imu, /odom y /cmd_vel a 10–20 Hz, con latencia de extremo a extremo (GPS → cmd_vel) < 100 ms.
• Uso de CPU en la Raspberry Pi 4 < 45 % y carga de GPU irrelevante (< 5 %, sin aceleración pesada), garantizando > 30 Hz de lazo de control.
• Transición limpia entre waypoints (sin oscilaciones fuertes ni cambios bruscos > 50 % en velocidad lineal/angular por ciclo).

Público objetivo: Estudiantes, makers y desarrolladores junior de robótica móvil; Nivel: Intermedio (se asume familiaridad básica con Linux, ROS 2 y cinemática diferencial).

Arquitectura/flujo: Nodo GPS lee /fix, se fusiona con IMU (MPU9250) y odometría en un filtro (p. ej. ekf_node) para generar /odom → nodo de navegación por waypoints calcula el error pose-objetivo, genera comandos de velocidad en /cmd_vel → controlador del UGV ejecuta la velocidad; monitorización desde RViz2 con TF entre map, odom y base_link.

Prerrequisitos

Sistema operativo y plataforma

• Hardware principal:
• Raspberry Pi 4 Model B (4 GB o 8 GB recomendado).
• Sistema operativo:
• Ubuntu Server 22.04 LTS 64-bit (aarch64) para Raspberry Pi 4.
• ROS 2:
• ROS 2 Humble Hawksbill (aarch64) instalado desde apt.

Versiones de toolchain

En este caso práctico usaremos:

• Kernel y SO (ejemplo típico):
• Ubuntu 22.04.4 LTS
• Kernel Linux 5.15.x para Raspberry Pi
• ROS 2:
• ros-humble-desktop (meta-paquete principal)
• Versiones instaladas por apt oficiales de Ubuntu.
• Compilador y herramientas:
• gcc 11.x
• cmake 3.22+
• python3 3.10
• colcon (python3-colcon-common-extensions)
• Librerías ROS 2 específicas:
• ros-humble-ros2-control
• ros-humble-diff-drive-controller
• ros-humble-robot-localization
• ros-humble-slam-toolbox
• ros-humble-nav2-bringup
• ros-humble-nav2-core
• ros-humble-nav2-costmap-2d
• ros-humble-nav2-planner
• ros-humble-nav2-controller
• ros-humble-nav2-lifecycle-manager
• ros-humble-rviz2

Nota: Aunque instalamos SLAM y Nav2 completos según la pauta, en este caso práctico usaremos principalmente robot_localization y un nodo propio de navegación por GPS.

Otros prerrequisitos

• Conocer comandos básicos de terminal: cd, ls, nano, vim.
• Saber usar ssh para acceder a la Raspberry Pi.
• Habilidad básica de lectura de pines GPIO e I2C desde documentación.

---

Materiales

Lista de materiales principales

• Computadora a bordo
• 1 × Raspberry Pi 4 Model B (4 GB o 8 GB RAM).
• Sensor de posicionamiento
• 1 × Adafruit Ultimate GPS HAT (MTK3339) para Raspberry Pi.
• Unidad de medida inercial
• 1 × Waveshare 10-DOF IMU Breakout
  • Acelerómetro + giroscopio: MPU9250
  • Presión/barómetro: BMP280
• Robot base UGV Beast (ROS 2) – RPi
• Chasis UGV Beast con motores de tracción diferencial.
• Controlador de motores (p. ej. puente H doble o driver de motor compatible).
• Batería adecuada para el UGV y la Raspberry Pi (ej. LiPo 3S con regulador 5 V).
• Cables y accesorios
• Cables dupont hembra-hembra para conexión I2C (Waveshare IMU → Raspberry Pi GPIO).
• Separadores/espaciadores para montar el GPS HAT sobre la Raspberry Pi.
• Tarjeta microSD (32 GB mínimo) para Ubuntu 22.04.
• Red
• Conexión Wi-Fi o Ethernet para acceso remoto a la Raspberry Pi.

---

Preparación y conexión

1. Instalación del sistema operativo y ROS 2 Humble

En tu PC:

1. Descargar imagen de Ubuntu Server 22.04 64-bit para Raspberry Pi desde la web oficial de Ubuntu.
2. Volcarla a la tarjeta microSD (con Raspberry Pi Imager o balenaEtcher).

En la Raspberry Pi (por consola, sin GUI):

Configurar Ubuntu 22.04 y ROS 2 Humble

# Actualizar sistema
sudo apt update
sudo apt upgrade -y

# Configurar locales (si aún no lo has hecho)
sudo apt install -y locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8

# Añadir repositorios ROS 2 Humble
sudo apt install -y software-properties-common
sudo add-apt-repository universe
sudo apt update

sudo apt install -y curl
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
  -o /usr/share/keyrings/ros-archive-keyring.gpg

echo "deb [arch=$(dpkg --print-architecture) \
signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" \
| sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

sudo apt update

# Instalar ROS 2 Humble desktop
sudo apt install -y ros-humble-desktop

# Paquetes adicionales requeridos
sudo apt install -y \
  ros-humble-ros2-control \
  ros-humble-diff-drive-controller \
  ros-humble-robot-localization \
  ros-humble-slam-toolbox \
  ros-humble-nav2-bringup \
  ros-humble-nav2-core \
  ros-humble-nav2-costmap-2d \
  ros-humble-nav2-planner \
  ros-humble-nav2-controller \
  ros-humble-nav2-lifecycle-manager \
  ros-humble-rviz2

# Herramientas de compilación
sudo apt install -y \
  python3-colcon-common-extensions \
  build-essential \
  cmake \
  git

Añadir source de ROS 2 al .bashrc:

echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc

2. Conexión de sensores a la Raspberry Pi

El Adafruit Ultimate GPS HAT se monta físicamente sobre la Raspberry Pi 4. El Waveshare 10-DOF IMU se conecta por I2C a los pines de la Raspberry Pi.

2.1. Adafruit Ultimate GPS HAT (MTK3339)

• Se inserta encima del GPIO de la Raspberry Pi, haciendo contacto con los 40 pines.
• Alimentación: toma 5 V directamente del conector.
• Comunicación principal: UART (ttyAMA0/ttyS0) y también acceso por I2C a RTC (si está presente, según modelo).

Configuración esencial:

• Habilitar UART en la Raspberry Pi.
• Deshabilitar la consola serie sobre el UART principal para usarlo con GPS.

En Ubuntu 22.04 para RPi (sin GUI), editar:

sudo nano /boot/firmware/cmdline.txt

Eliminar cualquier referencia a console=serial0,115200 o similar. Deja solo la consola por tty1.

Luego, habilitar UART en:

sudo nano /boot/firmware/config.txt

Añadir:

enable_uart=1

Guardar y reiniciar:

sudo reboot

Tras el reinicio, el dispositivo serie del GPS suele aparecer como /dev/ttyAMA0 o /dev/serial0. Compruébalo:

ls -l /dev/ttyAMA0 /dev/serial0

2.2. Waveshare 10-DOF IMU Breakout (MPU9250 + BMP280) por I2C

Conectar los pines:

Asegúrate de que la IMU esté configurada para 3.3 V en caso de jumper/selector.

Habilitar I2C en config.txt:

sudo nano /boot/firmware/config.txt

Añadir:

dtparam=i2c_arm=on

Reiniciar:

sudo reboot

Comprobar que I2C funciona:

sudo apt install -y i2c-tools
sudo i2cdetect -y 1

Deberías ver direcciones típicas como:

• 0x68 (MPU9250).
• 0x76 o 0x77 (BMP280).

---

Código completo: paquete ROS 2 para ros2-gps-waypoint-nav

Crearemos un workspace ~/ros2_ws con:

• Un paquete ugv_beast_description con URDF sencillo + ros2_control.
• Un paquete ugv_beast_bringup con lanzadores.
• Un paquete ros2_gps_waypoint_nav con un nodo Python para navegar waypoints GPS.

1. Crear el workspace y estructura

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws

# Paquete descripción
cd src
ros2 pkg create --build-type ament_cmake ugv_beast_description

# Paquete bringup
ros2 pkg create --build-type ament_cmake ugv_beast_bringup

# Paquete de navegación por GPS (Python)
ros2 pkg create --build-type ament_python ros2_gps_waypoint_nav

2. URDF y ros2_control (ugv_beast_description)

No entraremos en todos los detalles mecánicos del UGV Beast, pero definimos un modelo mínimo.

Crear directorios:

cd ~/ros2_ws/src/ugv_beast_description
mkdir -p urdf config

Archivo urdf/ugv_beast.urdf.xacro (simplificado, parte clave mostrada):

<?xml version="1.0"?>
<robot name="ugv_beast" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Parámetros físico-geométricos aproximados -->
  <xacro:property name="wheel_radius" value="0.1"/> <!-- 10 cm -->
  <xacro:property name="track_width"  value="0.35"/> <!-- distancia entre ruedas -->

  <link name="base_link">
    <inertial>
      <mass value="10.0"/>
      <origin xyz="0 0 0.1" rpy="0 0 0"/>
      <inertia ixx="0.5" iyy="0.5" izz="0.5"
               ixy="0.0" ixz="0.0" iyz="0.0"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0.1" rpy="0 0 0"/>
      <geometry>
        <box size="0.5 0.3 0.2"/>
      </geometry>
      <material name="gray">
        <color rgba="0.6 0.6 0.6 1.0"/>
      </material>
    </visual>
  </link>

  <!-- Rueda izquierda -->
  <link name="left_wheel_link"/>
  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <origin xyz="0 ${track_width/2} 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- Rueda derecha -->
  <link name="right_wheel_link"/>
  <joint name="right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel_link"/>
    <origin xyz="0 -${track_width/2} 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- Plugin ros2_control dif-drive -->
  <ros2_control name="ugv_beast_controller" type="system">
    <hardware>
      <plugin>ros2_control_hardware_interface/GenericSystem</plugin>
      <!-- En la práctica, aquí iría tu driver propio hacia el controlador de motores -->
    </hardware>
    <joint name="left_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="right_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="velocity"/>
    </joint>
  </ros2_control>

</robot>

Archivo config/diff_drive_controller.yaml:

diff_drive_controller:
  ros__parameters:
    use_sim_time: false
    publish_rate: 50.0
    base_frame_id: base_link
    odom_frame_id: odom
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]
    wheel_separation: 0.35    # track_width
    wheel_radius: 0.1
    cmd_vel_timeout: 0.5
    publish_wheel_data: true
    enable_odom_tf: true
    velocity_rolling_window_size: 10
    linear:
      x:
        has_velocity_limits: true
        max_velocity: 0.6
        min_velocity: -0.6
    angular:
      z:
        has_velocity_limits: true
        max_velocity: 1.5
        min_velocity: -1.5

Calibración rápida:
– Mide el diámetro de la rueda: radio = diámetro/2.
– Mide la distancia entre centros de ruedas: wheel_separation.
– Ajusta los valores en diff_drive_controller.yaml para que el odómetro no derive demasiado.

3. Configurar robot_localization (EKF IMU + odom)

En ugv_beast_bringup, crear config/ekf.yaml:

cd ~/ros2_ws/src/ugv_beast_bringup
mkdir -p config launch

Archivo config/ekf.yaml:

ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.1
    two_d_mode: true
    transform_time_offset: 0.0
    transform_timeout: 0.0
    publish_tf: true
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    # Entrada de odometría (p.ej. de diff_drive_controller /odom)
    odom0: /odom
    odom0_config: [true,  true,  false,
                   false, false, true,
                   false, false, false,
                   false, false, false,
                   false, false, false]
    odom0_differential: false
    odom0_relative: false

    # IMU (Waveshare 10-DOF) publicando en /imu/data
    imu0: /imu/data
    imu0_config: [false, false, false,
                  true,  true,  true,
                  false, false, false,
                  false, false, false,
                  false, false, false]
    imu0_differential: false
    imu0_relative: false
    imu0_remove_gravitational_acceleration: true

Ajusta los topics odom0 e imu0 a los que efectivamente generes con tus drivers de IMU y odometría. En este caso asumimos:
– /odom publicado por el diff_drive_controller.
– /imu/data publicado por un driver de la IMU Waveshare (no desarrollamos aquí el driver específico; puedes usar uno basado en MPU9250 + BMP280 en Python o C++ que publique sensor_msgs/msg/Imu).

4. Nodo de navegación por waypoints GPS (ros2_gps_waypoint_nav)

4.1. Estructura del paquete

En ros2_gps_waypoint_nav, edita package.xml y setup.cfg/setup.py para incluir dependencias básicas: rclpy, sensor_msgs, geometry_msgs, nav_msgs, std_msgs.

package.xml (fragmentos clave):

<package format="3">
  <name>ros2_gps_waypoint_nav</name>
  <version>0.0.1</version>
  <description>Navegación por waypoints GPS para UGV Beast (ROS2)</description>
  <maintainer email="you@example.com">Tu Nombre</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclpy</depend>
  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>nav_msgs</depend>
  <depend>std_msgs</depend>

  <exec_depend>python3</exec_depend>

</package>

setup.py:

from setuptools import setup

package_name = 'ros2_gps_waypoint_nav'

setup(
    name=package_name,
    version='0.0.1',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
         ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Tu Nombre',
    maintainer_email='you@example.com',
    description='Navegación por waypoints GPS usando ROS2 y UGV Beast',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'gps_waypoint_nav = ros2_gps_waypoint_nav.gps_waypoint_nav:main',
        ],
    },
)

Crear directorio de código:

cd ~/ros2_ws/src/ros2_gps_waypoint_nav
mkdir -p ros2_gps_waypoint_nav
touch ros2_gps_waypoint_nav/__init__.py

4.2. Lógica del nodo gps_waypoint_nav.py

Este nodo:

• Se suscribe a /fix (sensor_msgs/msg/NavSatFix) del GPS MTK3339 en el HAT.
• Se suscribe a /odometry/filtered (nav_msgs/msg/Odometry) para tener una mejor pose local.
• Convierte una lista de waypoints GPS (lat, lon) a un sistema local plano (aprox. ENU simple).
• Calcula la distancia y el ángulo hacia el siguiente waypoint respecto a la orientación actual.
• Publica comandos /cmd_vel (geometry_msgs/msg/Twist) para avanzar hacia el waypoint.
• Considera el waypoint alcanzado cuando la distancia es menor a un umbral (p. ej. 2 m).

Archivo ros2_gps_waypoint_nav/gps_waypoint_nav.py:

#!/usr/bin/env python3
import math
from typing import List, Tuple

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import NavSatFix
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy

# Utilidad: conversión simple lat/lon a sistema local (aprox UTM plano)
def geodetic_to_local(lat_ref, lon_ref, lat, lon):
    """
    Conversión aproximada de coordenadas geodésicas (lat, lon en grados)
    a coordenadas locales (x, y) en metros usando aproximación de equirectangular.
    Válido para recorridos cortos (< 1 km aprox.).
    """
    # Radio medio de la Tierra (m)
    R = 6378137.0
    # Convertir a radianes
    lat_ref_rad = math.radians(lat_ref)
    lat_rad = math.radians(lat)
    d_lat = lat_rad - lat_ref_rad
    d_lon = math.radians(lon - lon_ref)
    x = R * d_lon * math.cos(lat_ref_rad)
    y = R * d_lat
    return x, y


class GPSWaypointNavNode(Node):
    def __init__(self):
        super().__init__('gps_waypoint_nav')

        # Parámetros configurables
        self.declare_parameter('waypoints', [
            # Ejemplo: lista [lat1, lon1, lat2, lon2, ...]
            40.4168, -3.7038,  # Punto 1
            40.4169, -3.7035,  # Punto 2
            40.4170, -3.7032   # Punto 3
        ])
        self.declare_parameter('goal_tolerance', 2.0)        # en metros
        self.declare_parameter('linear_speed', 0.3)          # m/s
        self.declare_parameter('angular_speed_gain', 1.0)    # factor proporcional
        self.declare_parameter('max_angular_speed', 1.0)     # rad/s
        self.declare_parameter('fix_topic', '/fix')
        self.declare_parameter('odom_topic', '/odometry/filtered')
        self.declare_parameter('cmd_vel_topic', '/cmd_vel')

        # Cargar parámetros
        wp_list = self.get_parameter('waypoints').get_parameter_value().double_array_value
        if len(wp_list) % 2 != 0:
            self.get_logger().error('El parámetro "waypoints" debe tener longitud par (lat, lon)')
            wp_list = []

        self.waypoints: List[Tuple[float, float]] = []
        for i in range(0, len(wp_list), 2):
            self.waypoints.append((wp_list[i], wp_list[i+1]))

        self.goal_tolerance = self.get_parameter('goal_tolerance').get_parameter_value().double_value
        self.linear_speed = self.get_parameter('linear_speed').get_parameter_value().double_value
        self.angular_speed_gain = self.get_parameter('angular_speed_gain').get_parameter_value().double_value
        self.max_angular_speed = self.get_parameter('max_angular_speed').get_parameter_value().double_value

        fix_topic = self.get_parameter('fix_topic').get_parameter_value().string_value
        odom_topic = self.get_parameter('odom_topic').get_parameter_value().string_value
        cmd_vel_topic = self.get_parameter('cmd_vel_topic').get_parameter_value().string_value

        # Estado interno
        self.current_fix: NavSatFix = None
        self.current_yaw: float = 0.0
        self.local_ref_set: bool = False
        self.lat_ref: float = 0.0
        self.lon_ref: float = 0.0
        self.current_wp_idx: int = 0

        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=10
        )

        # Suscripciones
        self.fix_sub = self.create_subscription(
            NavSatFix, fix_topic, self.fix_callback, qos)

        self.odom_sub = self.create_subscription(
            Odometry, odom_topic, self.odom_callback, 10)

        # Publicador de velocidad
        self.cmd_vel_pub = self.create_publisher(Twist, cmd_vel_topic, 10)

        # Timer de control (10 Hz)
        self.control_timer = self.create_timer(0.1, self.control_loop)

        self.get_logger().info('Nodo gps_waypoint_nav inicializado.')
        self.get_logger().info(f'Waypoints cargados: {self.waypoints}')

    def fix_callback(self, msg: NavSatFix):
        self.current_fix = msg
        if not self.local_ref_set and msg.status.status >= 0:
            # Establecer referencia local en el primer fix válido
            self.lat_ref = msg.latitude
            self.lon_ref = msg.longitude
            self.local_ref_set = True
            self.get_logger().info(
                f'Referencia local establecida en lat={self.lat_ref}, lon={self.lon_ref}')

    def odom_callback(self, msg: Odometry):
        # Extraer yaw de la orientación cuaternión
        q = msg.pose.pose.orientation
        # Conversión quaternion -> yaw
        siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
        cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
        self.current_yaw = math.atan2(siny_cosp, cosy_cosp)

    def control_loop(self):
        # Condiciones mínimas
        if not self.waypoints:
            self.get_logger().warn_once('No hay waypoints configurados.')
            return

        if self.current_fix is None or not self.local_ref_set:
            self.get_logger().warn_once('Esperando a posición GPS válida...')
            return

        if self.current_wp_idx >= len(self.waypoints):
            # Ruta completada
            self.stop_robot()
            self.get_logger().info_once('Todos los waypoints alcanzados.')
            return

        # Waypoint objetivo actual
        target_lat, target_lon = self.waypoints[self.current_wp_idx]

        # Posición actual en sistema local (x, y)
        cur_x, cur_y = geodetic_to_local(
            self.lat_ref, self.lon_ref,
            self.current_fix.latitude, self.current_fix.longitude
        )

        # Posición objetivo en sistema local (x, y)
        goal_x, goal_y = geodetic_to_local(
            self.lat_ref, self.lon_ref,
            target_lat, target_lon
        )

        dx = goal_x - cur_x
        dy = goal_y - cur_y
        distance = math.hypot(dx, dy)

        # Comprobar si el waypoint está alcanzado
        if distance <= self.goal_tolerance:
            self.get_logger().info(
                f'Waypoint {self.current_wp_idx} alcanzado. Distancia={distance:.2f} m')
            self.current_wp_idx += 1
            # Parar brevemente
            self.stop_robot()
            return

        # Ángulo hacia el objetivo en el sistema local
        target_yaw = math.atan2(dy, dx)
        yaw_error = self.normalize_angle(target_yaw - self.current_yaw)

        # Control sencillo P de la orientación y avance constante
        twist = Twist()
        twist.linear.x = self.linear_speed
        twist.angular.z = self.angular_speed_gain * yaw_error

        # Saturar velocidad angular
        if twist.angular.z > self.max_angular_speed:
            twist.angular.z = self.max_angular_speed
        elif twist.angular.z < -self.max_angular_speed:
            twist.angular.z = -self.max_angular_speed

        self.cmd_vel_pub.publish(twist)

    def stop_robot(self):
        twist = Twist()
        self.cmd_vel_pub.publish(twist)

    @staticmethod
    def normalize_angle(angle):
        # Llevar ángulo a rango [-pi, pi]
        while angle > math.pi:
            angle -= 2.0 * math.pi
        while angle < -math.pi:
            angle += 2.0 * math.pi
        return angle


def main(args=None):
    rclpy.init(args=args)
    node = GPSWaypointNavNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.stop_robot()
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

Dar permisos de ejecución:

chmod +x ~/ros2_ws/src/ros2_gps_waypoint_nav/ros2_gps_waypoint_nav/gps_waypoint_nav.py

---

Compilación y ejecución

1. Compilar el workspace con colcon

cd ~/ros2_ws
colcon build

Tras compilar sin errores:

source install/setup.bash

Para que se haga siempre:

echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc

2. Lanzar los componentes requeridos

En una sesión tmux o varias terminales SSH, seguiremos una secuencia.

1. Lanzar driver GPS
   Puedes usar un nodo que publique sensor_msgs/NavSatFix en /fix a partir de /dev/ttyAMA0. Por ejemplo, usando nmea_navsat_driver (si lo instalas) o un script propio. Suponiendo que ya tienes un nodo publicando /fix:

bash
ros2 topic echo /fix # Para verificar

1. Lanzar IMU driver
   De forma análoga, lanza tu driver para MPU9250 + BMP280 que publique en /imu/data (tipo sensor_msgs/msg/Imu).

bash
ros2 topic echo /imu/data

1. Lanzar diff_drive_controller + robot_localization
   Crea un launch sencillo en ugv_beast_bringup/launch/ugv_beast_bringup.launch.py (esquema orientativo, sin mostrarlo entero aquí por extensión) que:

2. Cargue el URDF de ugv_beast_description.

3. Inicie controller_manager con diff_drive_controller.
4. Inicie el nodo ekf_node de robot_localization con ekf.yaml.

Ejecútalo (ejemplo):

bash
ros2 launch ugv_beast_bringup ugv_beast_bringup.launch.py

1. Lanzar el nodo ros2-gps-waypoint-nav

En otra terminal:

bash
ros2 run ros2_gps_waypoint_nav gps_waypoint_nav \
--ros-args \
-p waypoints:="[40.4168, -3.7038, 40.4169, -3.7035, 40.4170, -3.7032]" \
-p goal_tolerance:=2.0 \
-p linear_speed:=0.3 \
-p angular_speed_gain:=1.0 \
-p max_angular_speed:=0.8

Asegúrate de usar coordenadas GPS reales de tu entorno de prueba.

---

Validación paso a paso

1. Validar sensores

1. GPS HAT (MTK3339):

2. Comprobar dispositivo:

   bash
ls -l /dev/ttyAMA0
sudo cat /dev/ttyAMA0 | head

3. Debes ver mensajes NMEA ($GPGGA, $GPRMC, etc.).

4. En ROS 2, verifica el topic /fix:

   bash
ros2 topic list
ros2 topic echo /fix

5. Señales de éxito:

   • Frecuencia ≥ 1 Hz (ros2 topic hz /fix).
   • status.status >= 0 (fix 2D/3D).

6. IMU Waveshare 10-DOF:

7. i2cdetect -y 1 muestra direcciones del MPU9250 y BMP280.

8. En ROS 2:

   bash
ros2 topic echo /imu/data
ros2 topic hz /imu/data

9. Espera al menos 50 Hz si el driver está configurado a esa tasa.

2. Validar odometría y EKF

• Con diff_drive_controller activo:

bash
ros2 topic echo /odom
ros2 topic echo /odometry/filtered

• Mover ligeramente el robot (o ruedas en el aire) y verificar que las posiciones cambian coherentemente.
• Comprobar TF:

bash
ros2 run tf2_tools view_frames

Luego inspeccionar el PDF generado y verificar frames: odom -> base_link, map -> odom.

3. Validar /cmd_vel desde el nodo de navegación

• Antes de mover el robot en el suelo, haz una prueba con el robot levantado (ruedas en el aire) o sin motores energizados:

• Levanta el nodo de navegación.

• En otra terminal:

  bash
ros2 topic echo /cmd_vel

• Fija waypoints cercanos a tu posición actual.

• Comprueba que:
  • /cmd_vel tiene linear.x alrededor de 0.3.
  • angular.z cambia de signo según el ángulo hacia el objetivo.

4. Prueba de campo

1. Coloca el robot en un espacio abierto con buena visibilidad de cielo.
2. Espera a que el GPS consiga un fix estable (idealmente HDOP bajo, si expones este dato).
3. Mide las coordenadas del punto inicial (puedes leer /fix y anotar latitude/longitude).
4. Define 2–3 waypoints alrededor (p.ej. formar un triángulo de 10–15 m de lado).
5. Lanza todos los nodos como en la sección anterior.
6. Observa:
7. El robot se orienta hacia el primer waypoint y avanza.
8. Al entrar en el radio de tolerancia (~2 m), se detiene brevemente, cambia de orientación y se dirige al siguiente.
9. Completa la secuencia de waypoints con desviación ≤ 3–5 m respecto a cada punto (limitado por precisión GPS).

---

Troubleshooting (errores típicos y soluciones)

1. No aparece /fix en ROS 2
2. Causas probables:
   • UART está todavía ocupado por la consola serie.
   • Driver del GPS no está usando /dev/ttyAMA0 correcto.

3. Solución:

   • Revisa /boot/firmware/cmdline.txt y asegúrate de quitar console=serial0,....
   • Verifica enable_uart=1 en config.txt.
   • Confirma el dispositivo con ls -l /dev/ttyAMA0 /dev/serial0.

4. /imu/data no existe

5. Causas:
   • I2C no habilitado.
   • Cableado incorrecto (SDA/SCL invertidos o sin GND común).

6. Solución:

   • Habilita dtparam=i2c_arm=on.
   • Revisa que el Waveshare IMU 10-DOF esté alimentado a 3.3 V.
   • Verifica direcciones con i2cdetect -y 1.

7. El robot gira en círculos y no avanza hacia el waypoint

8. Causas:
   • Frame de referencia de odometría mal configurado.
   • Orientación (yaw) no coincide con el eje del robot.

9. Solución:

   • Comprueba que base_link está alineado con la dirección de avance del robot en el URDF.
   • Revisa TF (odom -> base_link) y confirmarlo en RViz.
   • Ajusta angular_speed_gain y max_angular_speed para evitar oscilaciones.

10. El robot se detiene aleatoriamente aunque aún no llegó al waypoint

11. Causas:
    • Pérdidas de señal GPS: status.status < 0 o saltos bruscos.
    • robot_localization no recibe datos a tiempo (time-out).

12. Solución:

    • Verifica calidad de señal GPS en un espacio más abierto.
    • Asegúrate de que la frecuencia de /fix sea estable.
    • Ajusta sensor_timeout en ekf.yaml si es demasiado bajo.

13. Error de compilación de ros2_gps_waypoint_nav

14. Causas:
    • setup.py o package.xml mal configurados.

15. Solución:

    • Revisa que packages=[package_name] en setup.py.
    • Verifica que __init__.py existe.
    • Asegúrate de que ros2_gps_waypoint_nav/gps_waypoint_nav.py es ejecutable.

16. colcon build no encuentra dependencias como rclpy

17. Causas:
    • ros-humble-desktop o python3-rclpy no instalados.

18. Solución:

    • Reinstala paquetes ROS 2 básicos:

    bash
sudo apt install -y ros-humble-desktop python3-rclpy

19. El robot avanza demasiado rápido o se sale del área de pruebas

20. Causas:
    • Parámetros de velocidad demasiado altos (linear_speed, max_angular_speed).

21. Solución:

    • Reduce linear_speed a 0.1–0.2 m/s.
    • Limita max_angular_speed a 0.5 rad/s.
    • Aumenta goal_tolerance si es necesario.

22. La ruta de waypoints no se completa (se queda en el waypoint 0)

23. Causas:
    • La lista waypoints no está bien formateada o el umbral de cercanía es muy pequeño.
24. Solución:
    • Revisa el parámetro waypoints: longitud par, en grados decimales.
    • Aumenta goal_tolerance a 3–4 m como prueba.

---

Mejoras y variantes

• Uso de Nav2 completo:
  Integrar el nodo gps_waypoint_nav con Nav2, convirtiendo cada waypoint GPS a un objetivo en el frame map y dejando que Nav2 planifique la trayectoria detallada.
• Planificación con obstáculos:
  Añadir un LiDAR (ej. RPLIDAR A1) y configurar slam_toolbox + costmaps de Nav2 para evitar obstáculos entre waypoints.
• Fusión de barómetro (BMP280):
  Integrar lecturas de altitud (presión) en robot_localization para entornos con cambios de altura moderados.
• Registro de datos:
  Usar ros2 bag record para guardar /fix, /imu/data, /odometry/filtered, /cmd_vel y analizar rutas recorridas.
• Interfaz de configuración de waypoints:
  Implementar un servicio ROS 2 que permita cambiar dinámicamente la lista de waypoints sin reiniciar el nodo.
• Modo “return-to-home”:
  Guardar la posición inicial y añadir un último waypoint que lleve al robot de vuelta a origen.

---

Checklist de verificación

Marca cada ítem cuando lo completes:

• [ ] Ubuntu 22.04 64-bit instalado y accesible por SSH en la Raspberry Pi 4 Model B.
• [ ] ROS 2 Humble instalado con ros-humble-desktop y paquetes adicionales (ros2-control, robot_localization, slam-toolbox, nav2*, rviz2).
• [ ] Workspace ~/ros2_ws creado y compilado con colcon build.
• [ ] Adafruit Ultimate GPS HAT (MTK3339) montado correctamente sobre la Raspberry Pi.
• [ ] UART habilitado (enable_uart=1) y consola serie deshabilitada en /boot/firmware/cmdline.txt.
• [ ] /dev/ttyAMA0 muestra datos NMEA y hay un nodo ROS 2 publicando en /fix (≥ 1 Hz).
• [ ] Waveshare 10-DOF IMU Breakout (MPU9250 + BMP280) conectado por I2C (pines SDA/SCL correctos).
• [ ] i2cdetect -y 1 detecta direcciones del MPU9250 y BMP280.
• [ ] Nodo IMU operativo publicando /imu/data a ≥ 50 Hz.
• [ ] URDF del robot cargado, diff_drive_controller operativo, publicando /odom.
• [ ] robot_localization configurado con ekf.yaml y publicando /odometry/filtered.
• [ ] Nodo gps_waypoint_nav se ejecuta sin errores y publica en /cmd_vel.
• [ ] En prueba estática, /cmd_vel cambia según la posición del waypoint.
• [ ] En prueba en campo, el robot recorre al menos 3 waypoints con error ≤ 3–5 m.
• [ ] Has aplicado al menos una mejora (p. ej. ajuste de velocidades, tolerancias, o logging de datos).

Con todo esto, habrás completado con éxito el caso práctico de ros2-gps-waypoint-nav sobre el UGV Beast (ROS 2) – Raspberry Pi 4 Model B + Adafruit Ultimate GPS HAT (MTK3339) + Waveshare 10-DOF IMU Breakout (MPU9250 + BMP280), logrando un sistema funcional de navegación básica por waypoints GPS con ROS 2 Humble.

Objetivo y caso de uso

Qué construirás: Un UGV Beast basado en ROS 2 Humble sobre Raspberry Pi 4 con HAT CAN MCP2515, teleoperado mediante joystick usando joy, teleop_twist_joy y diff_drive_controller enviando comandos de velocidad por bus CAN.

Para qué sirve

• Realizar pruebas de movilidad en laboratorio recorriendo pasillos de 5–15 m verificando tracción, curvas de 90° y distancia de frenado < 30 cm.
• Entrenar maniobras de precisión (giros en U en pasillos de 1 m, marcha atrás hasta una diana a < 5 cm de error).
• Generar trayectorias de referencia y mapas iniciales grabando cmd_vel y odometría para futuros algoritmos de navegación autónoma.
• Validar el hardware de locomoción comprobando que los motores responden a rampas de velocidad lineal 0–0.8 m/s y angular 0–1 rad/s sin fallos CAN.
• Usar el robot en demostraciones o clases, permitiendo control inmediato con joystick sin desarrollar lógica de navegación compleja.

Resultado esperado

• Teleoperación fluida con frecuencia estable de comandos cmd_vel de 20–50 Hz y latencia joystick->ruedas < 80 ms medida con ROS 2 (ros2 topic hz, ros2 topic delay).
• Uso de CPU en la Raspberry Pi < 35% y carga de bus CAN < 40% al mantener movimientos continuos (avance recto, giros, frenadas) durante sesiones de > 10 minutos.
• Respuesta estable del controlador diferencial: sin oscilaciones perceptibles y error en velocidad lineal < 10% respecto al comando durante tramos de 3–5 m.
• Capacidad de seguir un recorrido en pasillo de 10 m con dos giros de 90° sin pérdidas de comunicación CAN ni paradas inesperadas.

Público objetivo: Desarrolladores de robótica móvil, equipos de I+D y docentes; Nivel: Intermedio en ROS 2 y Linux embebido.

Arquitectura/flujo: Joystick USB conectado a estación de control → nodo joy en ROS 2 → nodo teleop_twist_joy genera geometry_msgs/Twist en /cmd_vel → diff_drive_controller en la Raspberry Pi convierte cmd_vel en velocidades de rueda → frame CAN a través del Waveshare 2-CH CAN HAT (MCP2515) → controladoras de motor del UGV Beast ejecutan los comandos.

Prerrequisitos

Sistema operativo y versiones

Se usará una configuración reproducible basada en línea de comandos (sin entorno gráfico):

• Placa principal: Raspberry Pi 4 Model B 4GB.
• SO en la Raspberry Pi:
• Ubuntu Server 22.04.5 LTS 64-bit (aarch64) para Raspberry Pi.
• Kernel típico: 5.15.x (el que instala Ubuntu 22.04 por defecto en Pi 4).
• ROS 2 en la Raspberry Pi:
• ROS 2 Humble Hawksbill (aarch64).
• Instalación por apt.

Toolchain exacta

En la Raspberry Pi 4:

• Compilador C++:
• g++ (Ubuntu 11.4.0-1ubuntu1~22.04) 11.4.0
• CMake:
• cmake version 3.22.1
• Colcon:
• python3-colcon-common-extensions versión de repositorios de Ubuntu 22.04.
• Python:
• Python 3.10.x (por defecto en Ubuntu 22.04).
• ROS 2 paquetes clave (instalación por apt):
• ros-humble-desktop
• ros-humble-ros2-control
• ros-humble-diff-drive-controller
• ros-humble-robot-localization
• ros-humble-slam-toolbox
• ros-humble-nav2-bringup
• ros-humble-nav2-simple-commander (normalmente incluido en nav2)
• ros-humble-rviz2
• ros-humble-joy
• ros-humble-teleop-twist-joy
• Herramientas CAN:
• can-utils 2021.08.0 (instalada desde apt como can-utils).

En el PC remoto (opcional, para joystick y RViz2), se recomienda:

• Ubuntu Desktop 22.04.5 LTS 64-bit.
• ROS 2 Humble instalado por apt igualmente.

---

Materiales

Lista de materiales principales

1. Computadora de a bordo:
2. Raspberry Pi 4 Model B 4GB (modelo exacto requerido).
3. HAT CAN:
4. Waveshare 2-CH CAN HAT (MCP2515), compatible con Raspberry Pi, interfaz SPI.
5. UGV Beast (base móvil):
6. Chasis UGV Beast con:
   • Controladora de motores con interfaz CAN (p. ej., un controlador propio del UGV o ESC con protocolo CAN).
   • Motores de tracción diferencial (rueda izquierda y derecha).
7. Alimentación:
8. Batería del UGV Beast (p. ej., LiPo 12 V o 24 V según especificación del robot).
9. Regulador DC-DC para alimentar la Raspberry Pi 4 con 5 V / 3 A (mínimo).
10. Conectividad:
11. Cable CAN (par trenzado, p. ej., cable con hilos verde/amarillo).
12. Conectores para interfaz CAN del UGV (según hardware del Beast).
13. Joystick USB:
14. Cualquier joystick o gamepad genérico reconocido por Linux (p. ej., Logitech F310, Xbox 360 Controller con cable).
15. Red:
16. Router WiFi o cable Ethernet para conectar:
    • Raspberry Pi 4
    • PC remoto (para lanzar teleop y RViz2 si se desea).
17. Almacenamiento:
18. Tarjeta microSD (32 GB, Clase 10 o superior) para Ubuntu 22.04.

---

Preparación y conexión

1. Conexión física del Waveshare 2-CH CAN HAT (MCP2515)

El HAT se monta directamente sobre la Raspberry Pi 4 mediante el conector de 40 pines (GPIO header). No se requieren cables intermedios para el bus SPI interno, ya que el HAT se alinea con el conector.

Los pines clave usados por el MCP2515 son:

En la práctica, al usar el HAT apilado, ya vienen todos esos pines cableados internamente.

2. Conexión CAN entre HAT y controladora del UGV Beast

El HAT Waveshare 2-CH CAN ofrece dos canales CAN (CAN0, CAN1). Para este caso práctico usaremos CAN0.

En el HAT, los terminales suelen etiquetarse como:

• CAN0_H y CAN0_L
• (Y otros dos para CAN1_H y CAN1_L)

Conecta:

• CAN0_H del HAT → CAN_H de la controladora del UGV Beast.
• CAN0_L del HAT → CAN_L de la controladora del UGV Beast.

Asegúrate de:

• Tener resistencia de terminación de 120 Ω en cada extremo del bus (a menudo el HAT tiene un jumper para activar esta resistencia; el UGV Beast puede tenerla integrada).
• Mantener el cable par trenzado para minimizar interferencias.

3. Alimentación

1. Alimenta la Raspberry Pi 4 con:
2. 5 V, 3 A mínimo (mediante USB-C o un regulador DC-DC desde la batería del UGV).
3. Alimenta la controladora de motores del UGV Beast según su especificación (p. ej., batería de 12 V o 24 V).
4. Asegúrate de que la tierra (GND) de la Raspberry Pi 4 y la controladora del UGV Beast estén comunes (normalmente se comparte por el bus CAN, pero verifica el diseño).

---

Código completo y configuración ROS 2

El objetivo del proyecto es ros2-joy-teleop-ugv: teleoperar el UGV Beast por joystick. Para ello:

1. Usaremos paquetes estándar:
2. joy (lectura del joystick).
3. teleop_twist_joy (convierte ejes/botones del joystick en geometry_msgs/Twist → cmd_vel).
4. diff_drive_controller (recibe cmd_vel y genera comandos de rueda).
5. Implementaremos un paquete propio que:
6. Defina el modelo URDF básico del UGV.
7. Configure ros2_control + diff_drive_controller.
8. Publique los comandos de rueda en un nodo CAN (C++) que hable con la controladora de motores por MCP2515.

1. Creación del workspace y paquete

En la Raspberry Pi 4:

# 1. Configurar ROS 2 Humble (tras instalarlo por apt)
sudo apt update
sudo apt install -y \
  ros-humble-desktop \
  ros-humble-ros2-control \
  ros-humble-diff-drive-controller \
  ros-humble-robot-localization \
  ros-humble-slam-toolbox \
  ros-humble-nav2-bringup \
  ros-humble-rviz2 \
  ros-humble-joy \
  ros-humble-teleop-twist-joy \
  can-utils

# 2. Sourcing inicial
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc

# 3. Crear workspace
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

# 4. Crear paquete para el UGV Beast
ros2 pkg create --build-type ament_cmake ugv_beast_teleop --dependencies rclcpp std_msgs geometry_msgs nav_msgs control_msgs hardware_interface pluginlib

2. Modelo URDF y ros2_control

En este nivel básico, definimos un modelo mínimo del UGV Beast con tracción diferencial:

• base_link como cuerpo principal.
• Dos ruedas: left_wheel_link y right_wheel_link.
• Parámetros mecánicos (ejemplo, ajustables en calibración):
• Radio de rueda: 0.1 m (10 cm).
• Distancia entre ruedas (track width): 0.4 m.

Crea un directorio urdf en el paquete:

cd ~/ros2_ws/src/ugv_beast_teleop
mkdir urdf config launch src

Archivo urdf/ugv_beast.urdf.xacro (o .urdf simple; aquí lo haremos en URDF directo para simplificar):

<?xml version="1.0"?>
<robot name="ugv_beast" xmlns:xacro="http://ros.org/wiki/xacro">

  <link name="base_link">
    <inertial>
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
      <mass value="10.0"/>
      <inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.8" iyz="0.0" izz="1.0"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
      <geometry>
        <box size="0.5 0.3 0.1"/>
      </geometry>
      <material name="gray">
        <color rgba="0.5 0.5 0.5 1.0"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
      <geometry>
        <box size="0.5 0.3 0.1"/>
      </geometry>
    </collision>
  </link>

  <link name="left_wheel_link">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.1"/>
      </geometry>
    </visual>
  </link>

  <link name="right_wheel_link">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.1"/>
      </geometry>
    </visual>
  </link>

  <joint name="left_wheel_joint" type="continuous">
    <origin xyz="0 0.2 0.0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

  <joint name="right_wheel_joint" type="continuous">
    <origin xyz="0 -0.2 0.0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="right_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- ros2_control interface -->
  <ros2_control name="ugv_beast_system" type="system">
    <hardware>
      <plugin>ugv_beast_teleop/CanDiffDriveHardware</plugin>
      <param name="can_interface">can0</param>
      <param name="wheel_radius">0.1</param>
      <param name="wheel_separation">0.4</param>
    </hardware>

    <joint name="left_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="velocity"/>
    </joint>

    <joint name="right_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="velocity"/>
    </joint>

    <controller name="diff_controllers" type="diff_drive_controller/DiffDriveController">
      <param name="left_wheel_names">["left_wheel_joint"]</param>
      <param name="right_wheel_names">["right_wheel_joint"]</param>
      <param name="wheel_separation">0.4</param>
      <param name="wheel_radius">0.1</param>
      <param name="publish_rate">50.0</param>
      <param name="cmd_vel_timeout">0.5</param>
      <param name="use_stamped_vel">false</param>
    </controller>
  </ros2_control>

</robot>

Nota: El plugin ugv_beast_teleop/CanDiffDriveHardware será nuestro hardware_interface que envía comandos por CAN.

3. Nodo de hardware CAN (C++): enviar comandos a la controladora del UGV

Crearemos un hardware_interface simple y un nodo que demuestre el envío de comandos por CAN. Para un nivel básico, enfocaremos el ejemplo a:

• Leer velocidades de rueda desde ros2_control.
• Convertirlas a tramas CAN (formato simplificado).
• Publicarlas en el bus can0 usando socketcan.

Archivo src/can_diff_drive_hardware.cpp (fragmento principal):

#include <rclcpp/rclcpp.hpp>
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/handle.hpp>
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <pluginlib/class_list_macros.hpp>

#include <linux/can.h>
#include <linux/can/raw.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <cstring>
#include <unistd.h>

using CallbackReturn = hardware_interface::CallbackReturn;

namespace ugv_beast_teleop
{

class CanDiffDriveHardware : public hardware_interface::SystemInterface
{
public:
  CanDiffDriveHardware() = default;

  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
  {
    if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
      return CallbackReturn::ERROR;
    }

    wheel_radius_ = std::stod(info.hardware_parameters.at("wheel_radius"));
    wheel_separation_ = std::stod(info.hardware_parameters.at("wheel_separation"));
    can_interface_ = info.hardware_parameters.at("can_interface");

    // Inicializar socket CAN
    if ((can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
      RCLCPP_ERROR(rclcpp::get_logger("CanDiffDriveHardware"), "Error creating CAN socket");
      return CallbackReturn::ERROR;
    }

    struct ifreq ifr;
    std::strncpy(ifr.ifr_name, can_interface_.c_str(), IFNAMSIZ - 1);
    if (ioctl(can_socket_, SIOCGIFINDEX, &ifr) < 0) {
      RCLCPP_ERROR(rclcpp::get_logger("CanDiffDriveHardware"), "CAN interface %s not found", can_interface_.c_str());
      return CallbackReturn::ERROR;
    }

    struct sockaddr_can addr{};
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;

    if (bind(can_socket_, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
      RCLCPP_ERROR(rclcpp::get_logger("CanDiffDriveHardware"), "Error binding CAN socket");
      return CallbackReturn::ERROR;
    }

    // Inicializar vectores de estados y comandos
    hw_states_.resize(2, 0.0);
    hw_commands_.resize(2, 0.0);

    return CallbackReturn::SUCCESS;
  }

  std::vector<hardware_interface::StateInterface> export_state_interfaces() override
  {
    std::vector<hardware_interface::StateInterface> interfaces;
    interfaces.emplace_back(hardware_interface::StateInterface(
      "left_wheel_joint", hardware_interface::HW_IF_VELOCITY, &hw_states_[0]));
    interfaces.emplace_back(hardware_interface::StateInterface(
      "right_wheel_joint", hardware_interface::HW_IF_VELOCITY, &hw_states_[1]));
    return interfaces;
  }

  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override
  {
    std::vector<hardware_interface::CommandInterface> interfaces;
    interfaces.emplace_back(hardware_interface::CommandInterface(
      "left_wheel_joint", hardware_interface::HW_IF_VELOCITY, &hw_commands_[0]));
    interfaces.emplace_back(hardware_interface::CommandInterface(
      "right_wheel_joint", hardware_interface::HW_IF_VELOCITY, &hw_commands_[1]));
    return interfaces;
  }

  hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override
  {
    // En este ejemplo básico, asumimos que no leemos estados reales (odometría directa del motor).
    // Simplemente copiamos comandos a estados para tener algo coherente.
    hw_states_[0] = hw_commands_[0];
    hw_states_[1] = hw_commands_[1];
    return hardware_interface::return_type::OK;
  }

  hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override
  {
    // Convertir velocidades de rueda (rad/s) a mensaje CAN según protocolo del UGV Beast.
    // Ejemplo simple: ID 0x100, 4 bytes: int16_t left, int16_t right (en centi-rad/s).

    struct can_frame frame{};
    frame.can_id = 0x100;
    frame.can_dlc = 4;

    int16_t left = static_cast<int16_t>(hw_commands_[0] * 100.0);
    int16_t right = static_cast<int16_t>(hw_commands_[1] * 100.0);

    frame.data[0] = left & 0xFF;
    frame.data[1] = (left >> 8) & 0xFF;
    frame.data[2] = right & 0xFF;
    frame.data[3] = (right >> 8) & 0xFF;

    int nbytes = write_can_frame(frame);
    if (nbytes != sizeof(struct can_frame)) {
      RCLCPP_WARN(rclcpp::get_logger("CanDiffDriveHardware"), "Failed to send full CAN frame");
    }

    return hardware_interface::return_type::OK;
  }

private:
  int write_can_frame(const struct can_frame &frame)
  {
    return ::write(can_socket_, &frame, sizeof(frame));
  }

  double wheel_radius_{0.1};
  double wheel_separation_{0.4};
  std::string can_interface_{"can0"};
  int can_socket_{-1};

  std::vector<double> hw_states_;
  std::vector<double> hw_commands_;
};

}  // namespace ugv_beast_teleop

PLUGINLIB_EXPORT_CLASS(ugv_beast_teleop::CanDiffDriveHardware, hardware_interface::SystemInterface)

Este código:

• Implementa un SystemInterface para ros2_control.
• Abre un socket CAN sobre la interfaz can0.
• Envía una trama CAN con las velocidades de rueda codificadas como enteros de 16 bits (formato de ejemplo; deberías adaptarlo al protocolo real de tu UGV Beast).

4. CMakeLists.txt y package.xml

En CMakeLists.txt añade:

cmake_minimum_required(VERSION 3.8)
project(ugv_beast_teleop)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(control_msgs REQUIRED)

include_directories(
  include
)

add_library(can_diff_drive_hardware SHARED
  src/can_diff_drive_hardware.cpp
)

target_link_libraries(can_diff_drive_hardware
  ${rclcpp_LIBRARIES}
)

ament_target_dependencies(can_diff_drive_hardware
  rclcpp
  hardware_interface
  pluginlib
)

pluginlib_export_plugin_description_file(hardware_interface hardware_plugins.xml)

install(TARGETS
  can_diff_drive_hardware
  LIBRARY DESTINATION lib
)

install(FILES
  urdf/ugv_beast.urdf.xacro
  DESTINATION share/${PROJECT_NAME}/urdf
)

install(FILES
  hardware_plugins.xml
  DESTINATION share/${PROJECT_NAME}
)

ament_package()

Crea el archivo hardware_plugins.xml en el directorio raíz del paquete:

<library path="libcan_diff_drive_hardware.so">
  <class
    type="ugv_beast_teleop::CanDiffDriveHardware"
    base_class_type="hardware_interface::SystemInterface">
    <description>CAN-based diff drive hardware interface for UGV Beast.</description>
  </class>
</library>

En package.xml, asegúrate de tener:

<package format="3">
  <name>ugv_beast_teleop</name>
  <version>0.0.1</version>
  <description>Teleoperación UGV Beast por joystick con ROS 2 y CAN.</description>
  <maintainer email="tu_email@example.com">Tu Nombre</maintainer>
  <license>MIT</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>hardware_interface</depend>
  <depend>pluginlib</depend>
  <depend>geometry_msgs</depend>
  <depend>nav_msgs</depend>
  <depend>control_msgs</depend>

  <export>
    <build_type>ament_cmake</build_type>
    <hardware_interface plugin="${prefix}/hardware_plugins.xml"/>
  </export>
</package>

5. Configuración de joy y teleop_twist_joy

En el PC remoto (o en la propia Raspberry Pi si conectas el joystick allí):

1. Conecta el joystick USB.
2. Instala paquetes (si no lo hiciste):

sudo apt install -y ros-humble-joy ros-humble-teleop-twist-joy

1. Crea un archivo de configuración YAML (por ejemplo en el PC) para mapear ejes/botones: ~/ros2_ws/src/ugv_beast_teleop/config/teleop_joy.yaml:

teleop_twist_joy_node:
  ros__parameters:
    axis_linear.x: 1
    scale_linear.x: 0.5
    axis_angular.z: 0
    scale_angular.z: 1.0
    enable_button: 4  # LB en mandos tipo Xbox
    enable_turbo_button: 5

Con esto:

• Eje 1 (normalmente joystick izquierdo vertical) controla la velocidad lineal.
• Eje 0 (joystick izquierdo horizontal) controla velocidad angular (ajústalo según tu joystick).
• Botón 4 debe mantenerse pulsado para enviar cmd_vel.

---

Compilación y ejecución

1. Configurar interfaz CAN (MCP2515) en Raspberry Pi 4

Edita /boot/firmware/config.txt en Ubuntu 22.04 para la Pi:

sudo nano /boot/firmware/config.txt

Añade:

dtoverlay=mcp2515-can0,oscillator=16000000,interrupt=25
dtoverlay=spi0-hw-cs
dtparam=spi=on

Guarda, sal y reinicia:

sudo reboot

Tras reiniciar, configura la interfaz CAN a 500 kbit/s (ajusta a la velocidad de tu bus):

sudo ip link set can0 up type can bitrate 500000
ip -details link show can0

Verifica que can0 está UP.

2. Compilar el workspace en la Raspberry Pi 4

cd ~/ros2_ws
colcon build --symlink-install
source install/setup.bash

3. Lanzar robot_state_publisher y diff_drive_controller

Crea un launch simple launch/ugv_beast_bringup.launch.py:

from launch import LaunchDescription
from launch_ros.actions import Node

from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    pkg_share = get_package_share_directory('ugv_beast_teleop')
    urdf_file = os.path.join(pkg_share, 'urdf', 'ugv_beast.urdf.xacro')

    with open(urdf_file, 'r') as infp:
        robot_desc = infp.read()

    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        parameters=[{'robot_description': robot_desc}]
    )

    diff_drive_controller_node = Node(
        package='controller_manager',
        executable='ros2_control_node',
        parameters=[{'robot_description': robot_desc}],
        output='screen'
    )

    spawner_diff = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['diff_controllers'],
        output='screen'
    )

    return LaunchDescription([
        robot_state_publisher_node,
        diff_drive_controller_node,
        spawner_diff
    ])

Asegúrate de tener robot_state_publisher instalado:
sudo apt install -y ros-humble-robot-state-publisher

Lanza desde la Raspberry Pi:

cd ~/ros2_ws
source install/setup.bash
ros2 launch ugv_beast_teleop ugv_beast_bringup.launch.py

4. Lanzar joy y teleop_twist_joy

En el PC remoto (o en la Pi si usas el joystick allí), en una terminal:

source /opt/ros/humble/setup.bash
ros2 run joy joy_node

En otra terminal, usando tu archivo de configuración:

source /opt/ros/humble/setup.bash
ros2 run teleop_twist_joy teleop_node --ros-args \
  --params-file ~/ros2_ws/src/ugv_beast_teleop/config/teleop_joy.yaml \
  -r cmd_vel:=/diff_controllers/cmd_vel_unstamped

Ajustamos el remapeo para que teleop_twist_joy publique en el tema que espera diff_drive_controller (cmd_vel_unstamped si use_stamped_vel=false).

---

Validación paso a paso

1. Comprobación de ROS 2 en Raspberry Pi

En la Pi, con el bringup corriendo, abre otra terminal:

source ~/ros2_ws/install/setup.bash
ros2 topic list

Debes ver (entre otros):

• /diff_controllers/cmd_vel_unstamped
• /joint_states
• /tf
• /tf_static

Ver estados de las ruedas:

ros2 topic echo /joint_states

Aunque no haya odometría real, deberías ver cambios cuando envíes comandos.

2. Verificar joy y cmd_vel en el PC remoto

Con joy_node y teleop_twist_joy lanzados:

1. Verifica que el joystick es reconocido:

bash
ros2 topic echo /joy

• Mueve los joysticks y pulsa botones: deberían cambiar los arreglos axes y buttons.

• Verifica que, con el botón de habilitación pulsado, se publica cmd_vel:

bash
ros2 topic echo /diff_controllers/cmd_vel_unstamped

• Mueve el joystick lineal: verás linear.x variando.
• Mueve el joystick angular: verás angular.z variando.

3. Validar comandos CAN en la Raspberry Pi

En la Raspberry Pi (otra terminal):

sudo apt install -y can-utils
candump can0

Mientras mueves el joystick:

• Deberían aparecer tramas CAN con ID 0x100 (según nuestro ejemplo).
• La frecuencia de aparición se corresponde con la publish_rate del controlador y la tasa de cmd_vel (~20–30 Hz).

Métrica básica:

• Si mantienes el joystick en posición fija, deberías ver un flujo estable de tramas CAN sin errores en consola.

4. Validar movimiento físico del UGV Beast

Con el UGV en el suelo y espacio seguro alrededor:

1. Pulsa el botón de habilitación en el joystick.
2. Empuja el joystick hacia adelante:
3. El UGV debe avanzar en línea recta.
4. Recuerda nuestro ejemplo: scale_linear.x: 0.5 → velocidad máxima 0,5 m/s (teórica).
5. Gira el joystick hacia la derecha/izquierda:
6. El UGV debe rotar en su lugar o trazar una curva.

Métricas aproximadas:

• Avance de 1 m:
• Mantén el joystick a la mitad de recorrido hacia adelante durante 4 s.
• Si el radio de rueda y separación están bien calibrados, la distancia recorrida debería ser ≈1 m (±10 %).

---

Troubleshooting (errores típicos y soluciones)

1. No aparece can0 tras reiniciar

2. Revisa /boot/firmware/config.txt:

   • Asegúrate de haber escrito:
   • dtoverlay=mcp2515-can0,oscillator=16000000,interrupt=25
   • dtparam=spi=on

3. Comprueba módulos cargados:

   bash
lsmod | grep mcp251x

4. Verifica si el dispositivo SPI está habilitado:

   bash
ls /dev/spidev0.*

5. Si no aparece, revisa que el HAT está correctamente apilado sobre la Pi y sin pines doblados.

6. ip link set can0 up da error

7. Mensaje típico: Cannot find device "can0".

8. Solución:
   • Usa dmesg | grep -i mcp2515 para ver mensajes del kernel.
   • Verifica la velocidad: asegúrate de que el bus CAN está correctamente cableado y con terminación si hay otro nodo activo.

9. Si hay conflicto de direcciones de interrupción, revisa el pin INT (GPIO25) y que no esté siendo usado por otro periférico.

10. No se reciben tramas en candump can0

11. Verifica que:

    • Estás moviendo el joystick con el botón de habilitación pulsado.
    • teleop_twist_joy está remapeando a /diff_controllers/cmd_vel_unstamped.
    • diff_drive_controller está activo (ros2 control list_controllers).

12. Prueba a enviar una trama de prueba manual:

    bash
cansend can0 100#01000200

13. Si tampoco se ve, comprueba la conexión física con la controladora del UGV o con otro nodo CAN conocido.

14. El UGV se mueve al revés (adelante/atrás invertido)

15. Síntoma:

    • Mueves joystick hacia adelante y el UGV va hacia atrás.

16. Solución:

    • Invierte el signo de la rueda correspondiente en write() de can_diff_drive_hardware.cpp:

    cpp
int16_t left = static_cast<int16_t>(-hw_commands_[0] * 100.0);
int16_t right = static_cast<int16_t>(-hw_commands_[1] * 100.0);

    • O intercambia las ruedas izquierda/derecha según necesidad (modificando el orden de comandos).

17. El UGV gira más de un lado que de otro (errores de calibración)

18. Causa probable:

    • Valores incorrectos de wheel_radius y wheel_separation.

19. Pasos de calibración básica:

    1. Mide el diámetro real de las ruedas y calcula radius = diámetro / 2.
    2. Mide con precisión la distancia entre centros de ruedas (wheel_separation).
    3. Actualiza estos parámetros tanto en:
       • <hardware> del URDF.
       • <controller> del diff_drive_controller.
    4. Reconstruye el workspace y prueba nuevamente.

20. No hay cmd_vel aunque joy publica valores

21. Verifica:

    • ros2 topic echo /joy → debe cambiar.
    • Configuración YAML de teleop_twist_joy:
    • enable_button debe coincidir con un botón real (comprueba en el echo).
    • Ejes (axis_linear.x, axis_angular.z) deben corresponder a los ejes correctos.

22. Comprueba remapeo:

    bash
ros2 topic list | grep cmd_vel

23. Si teleop_twist_joy publica en /cmd_vel pero diff_drive_controller escucha en /diff_controllers/cmd_vel_unstamped, ajusta el remapeo (-r cmd_vel:=/diff_controllers/cmd_vel_unstamped).

24. Error de plugin en ros2_control_node

25. Mensaje típico:

    • Could not load library libcan_diff_drive_hardware.so

26. Soluciones:

    • Asegúrate de que hardware_plugins.xml y la definición pluginlib_export_plugin_description_file están bien configurados.
    • Reconstruye:

    bash
cd ~/ros2_ws
colcon build --symlink-install --packages-select ugv_beast_teleop
source install/setup.bash

    • Verifica que el nombre de la librería en hardware_plugins.xml coincide con la generada (libcan_diff_drive_hardware.so).

27. Latencia alta entre joystick y movimiento

28. Posibles causas:

    • Red WiFi saturada o con mala señal.
    • publish_rate del controlador demasiado baja.
29. Acciones:
    • Usa Ethernet en vez de WiFi entre PC y Raspberry Pi, si es posible.
    • Aumenta publish_rate en los parámetros del diff_drive_controller a 50–100 Hz.
    • Verifica uso de CPU en la Raspberry (top, htop) y cierra procesos innecesarios.

---

Mejoras y variantes

1. Integrar IMU y robot_localization

2. Añade una IMU al UGV Beast (por I2C, SPI o CAN).

3. Configura ekf_node de robot_localization para fusionar:
   • wheel_odom (de diff_drive_controller).
   • imu/data.

4. Ventaja: odometría más estable y robusta a resbalamientos.

5. Añadir LiDAR y SLAM

6. Instalar un LiDAR (p. ej. RPLIDAR A1) conectado por USB a la Raspberry Pi.

7. Usar rplidar_ros + slam_toolbox para crear mapas en línea.

8. Teleoperar el UGV mientras SLAM genera el mapa, y luego guardarlo con map_server.

9. Navegación semiautónoma (Nav2)

10. Con mapa y odometría configurados, emplear nav2 para:

    • Enviar objetivos de navegación (2D Nav Goal en RViz2).
    • Usar teleop sólo para maniobras especiales o emergencias.

11. Seguridad adicional

12. Implementar un botón de parada de emergencia (E-STOP) que:

    • Corte alimentación de motores.
    • O envíe tramas CAN de parada inmediata.

13. Añadir supervisión de tiempo:

    • Si no se reciben cmd_vel durante un tiempo, detener el UGV automáticamente.

14. Teleoperación desde tablets o gamepads inalámbricos

15. Usar rosbridge_suite para controlar el UGV con una interfaz web.

16. Integrar mando inalámbrico (Bluetooth) con el PC remoto.

---

Checklist de verificación

Puedes usar esta lista como autoevaluación (marca mentalmente o en papel):

• [ ] Ubuntu Server 22.04 64-bit instalado y funcionando en Raspberry Pi 4 Model B 4GB.
• [ ] ROS 2 Humble instalado por apt en la Raspberry Pi (ros-humble-desktop y paquetes adicionales).
• [ ] HAT Waveshare 2-CH CAN HAT (MCP2515) correctamente montado sobre la Raspberry Pi.
• [ ] Configuración SPI y MCP2515 en /boot/firmware/config.txt correcta (reinicio hecho).
• [ ] Interfaz can0 visible y subida con ip link set can0 up type can bitrate 500000.
• [ ] Workspace ~/ros2_ws creado con el paquete ugv_beast_teleop.
• [ ] Modelo URDF mínimo del UGV Beast creado con base_link y ruedas izquierda/derecha.
• [ ] Plugin CanDiffDriveHardware implementado, compilado y exportado en hardware_plugins.xml.
• [ ] ros2_control_node y diff_drive_controller lanzados sin errores desde la Raspberry Pi.
• [ ] Joystick USB reconocido en el PC remoto (ros2 topic echo /joy funciona).
• [ ] teleop_twist_joy configurado y publicando en /diff_controllers/cmd_vel_unstamped.
• [ ] Tramas CAN observadas con candump can0 cuando se mueve el joystick.
• [ ] El UGV Beast se mueve en la dirección esperada al accionar el joystick (adelante/atrás y giros).
• [ ] Latencia de control aceptable (< 0,3 s percibida).
• [ ] Sin errores críticos persistentes en los logs (ros2 y dmesg).

Si todos los elementos de la checklist se cumplen, has completado con éxito el caso práctico ros2-joy-teleop-ugv para el UGV Beast (ROS 2) – RPi con Raspberry Pi 4 Model B 4GB + Waveshare 2-CH CAN HAT (MCP2515).

Objetivo y caso de uso

Qué construirás: Un UGV tipo Beast con Raspberry Pi 4 y cámara Arducam OV5647, ejecutando ROS 2 Humble y un nodo propio ros2-line-follower-camera para seguir una línea negra en el suelo.

Para qué sirve

• Automatizar un robot de reparto interno que sigue una ruta marcada en el suelo dentro de un almacén.
• Guiar un vehículo educativo en circuitos de competición de seguidores de línea, con curvas suaves y cambios moderados de iluminación.
• Implementar rutas predefinidas en laboratorios o fábricas para mover piezas ligeras entre estaciones sin usar sensores adicionales.
• Prototipar AGVs de logística interna que se orientan únicamente con visión monocular (cámara frontal) y una línea en el piso.
• Practicar visión artificial básica aplicada a robótica móvil en ROS 2, incluyendo publicación de imágenes y control de velocidad.

Resultado esperado

• Mantener el centro del robot dentro de un corredor de ±5 cm respecto al centro de la línea durante al menos el 90 % del recorrido en un circuito simple.
• Publicar imágenes de la cámara en /camera/image_raw a ≥ 15 FPS con carga de CPU < 70 % en la Raspberry Pi 4.
• Publicar comandos de velocidad en /cmd_vel desde ros2-line-follower-camera a ≥ 10 Hz, con latencia desde captura de imagen hasta comando < 100 ms.
• Completar un circuito cerrado de prueba (p. ej., 10–20 m con 3–4 curvas amplias) sin salirse de la línea más de 2 veces por vuelta.

Público objetivo: Estudiantes de robótica, makers y desarrolladores que comienzan con ROS 2 en plataformas móviles; Nivel: Inicial–intermedio en ROS 2 y Linux, básico en visión por computador.

Arquitectura/flujo: La cámara Arducam OV5647 se conecta a la Raspberry Pi 4, que ejecuta un nodo de cámara (publica en /camera/image_raw) y el nodo ros2-line-follower-camera; este nodo procesa cada frame, detecta la posición de la línea, calcula el error lateral y el ángulo de la línea, los traduce en comandos de velocidad lineal y angular que publica en /cmd_vel, y el controlador del UGV aplica dichos comandos a los motores en tiempo casi real.

Prerrequisitos (SO, versiones y toolchain concreta)

Sistema operativo y hardware

• Plataforma principal:
• Raspberry Pi 4 Model B 4GB (aarch64)
• Sistema operativo:
• Ubuntu Server 22.04 LTS 64‑bit para Raspberry Pi (aarch64), sin entorno gráfico.
• Conectividad:
• Acceso SSH a la Raspberry Pi desde un PC en la misma red (para editar, compilar y probar).

Toolchain y versiones

En la Raspberry Pi (aarch64):

• ROS 2:
• Distribución: ROS 2 Humble Hawksbill
• Paquetes instalados vía apt:
• ros-humble-desktop
• ros-humble-ros2-control
• ros-humble-diff-drive-controller
• ros-humble-robot-localization
• ros-humble-slam-toolbox
• ros-humble-nav2-bringup
• ros-humble-nav2-costmap-2d
• ros-humble-nav2-controller
• ros-humble-nav2-planner
• ros-humble-nav2-behavior-tree
• ros-humble-nav2-lifecycle-manager
• ros-humble-rviz2
• Lenguaje principal para el nodo seguidor de línea:
• Python 3.10 (incluido en Ubuntu 22.04)
• Compilación:
• colcon versión instalada desde repositorios de Ubuntu (python3-colcon-common-extensions)
• Gestión de paquetes:
• apt (Ubuntu)
• Control de versiones (opcional pero recomendado):
• git ≥ 2.34

---

Materiales

Lista de materiales principales

• 1 × Raspberry Pi 4 Model B 4GB + Arducam 5MP OV5647 Camera Module (modelo exacto solicitado).
• 1 × Tarjeta microSD (32 GB o más, clase 10), con Ubuntu Server 22.04 64‑bit instalado.
• 1 × Chasis de robot tipo UGV Beast con:
• 2 motores DC con encoder (tracción diferencial).
• 2 ruedas motrices + rueda loca.
• Controlador de motores (por ejemplo, un driver tipo L298N o un hat específico para la plataforma Beast).
• 1 × Fuente de alimentación/batería adecuada para el UGV (por ejemplo, LiPo 2S–3S con BEC o banco de baterías con regulación).
• 1 × Cable USB para depuración (si el chasis incluye microcontrolador adicional).
• 1 × Conexión a red (Ethernet o Wi-Fi configurado en la Raspberry Pi).
• Cables Dupont macho-macho / macho-hembra para la conexión GPIO (si el driver de motores se controla desde la Pi).

Material opcional (pero útil)

• Regla o metro para medir desplazamientos reales y calibrar el robot.
• Cinta aislante negra para marcar la línea en el suelo.
• Cartulina blanca o espuma EVA blanca como fondo de alto contraste.

---

Preparación y conexión

1. Preparación del sistema operativo y ROS 2 Humble

1. Instala Ubuntu Server 22.04 64‑bit (aarch64) en la microSD (por ejemplo con Raspberry Pi Imager).
2. Arranca la Raspberry Pi 4 con la microSD y conéctala a red.
3. Actualiza el sistema:

bash
sudo apt update
sudo apt upgrade -y

1. Instala ROS 2 Humble y paquetes necesarios:

bash
sudo apt install -y \
ros-humble-desktop \
ros-humble-ros2-control \
ros-humble-diff-drive-controller \
ros-humble-robot-localization \
ros-humble-slam-toolbox \
ros-humble-nav2-bringup \
ros-humble-nav2-costmap-2d \
ros-humble-nav2-controller \
ros-humble-nav2-planner \
ros-humble-nav2-behavior-tree \
ros-humble-nav2-lifecycle-manager \
ros-humble-rviz2 \
python3-colcon-common-extensions \
python3-argcomplete \
git

1. Añade el setup.bash de ROS 2 a tu .bashrc:

bash
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc

2. Habilitar la cámara Arducam 5MP OV5647 en Raspberry Pi 4

En Ubuntu Server 22.04 para Raspberry Pi, la cámara se habilita mediante:

1. Editar /boot/firmware/config.txt:

bash
sudo nano /boot/firmware/config.txt

1. Asegúrate de tener estas líneas (y sin #):

text
start_x=1
gpu_mem=128
dtoverlay=imx219 # En algunas imágenes se usa este overlay genérico para cámaras; revisar documentación Arducam

Nota: Algunas versiones de firmware usan dtoverlay=ov5647. Si tu módulo Arducam 5MP OV5647 tiene documentación que especifica este overlay, usa:

text
dtoverlay=ov5647

1. Guarda y reinicia:

bash
sudo reboot

1. Verifica que el dispositivo de cámara está presente (según kernel/driver, puede ser /dev/video0):

bash
ls /dev/video*

Debe aparecer algo como /dev/video0.

3. Conexión física de la cámara al Raspberry Pi 4

La Arducam 5MP OV5647 Camera Module se conecta mediante el conector CSI:

• En la Raspberry Pi 4 hay un conector FPC (flex cable) cerca del conector HDMI.
• Pasos (sin electricidad):
• Apaga la Raspberry Pi y desconecta la alimentación.
• Levanta suavemente la pestaña negra del conector CSI.
• Inserta el cable plano de la Arducam con los contactos metálicos apuntando hacia los contactos del conector de la Pi.
• Vuelve a bajar la pestaña hasta fijar el cable.
• Conecta la alimentación y arranca.

4. Conexión del controlador de motores al Raspberry Pi 4

No dibujaremos esquema, pero describimos la conexión lógica típica (ejemplo con driver tipo L298N y motores DC):

Importante: Asegúrate de que la masa (GND) de la Raspberry Pi y la del driver de motores está unida. Si tu chasis UGV Beast trae su propia placa controladora, adapta estos pines según la documentación del fabricante, pero mantén la lógica de “2 GPIO de dirección + 1 PWM por motor”.

---

Código completo del seguidor de línea con cámara

Crearemos un paquete ROS 2 en Python dentro de un workspace ~/ros2_ws. Este paquete:

• Se llamará ros2_line_follower_camera.
• Tendrá un nodo line_follower_node.py que:
• Suscribe al tópico de la cámara /camera/image_raw.
• Usa OpenCV para procesar la imagen, detectar la línea negra en el suelo.
• Calcula un error horizontal (distancia del centro de la línea al centro de la imagen).
• Genera comandos de velocidad (lineal y angular) en /cmd_vel para un robot diferencial controlado por diff_drive_controller.

1. Crear el workspace y el paquete

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

ros2 pkg create --build-type ament_python ros2_line_follower_camera

Instala dependencias necesarias para procesamiento de imágenes:

sudo apt install -y python3-opencv python3-numpy

Edita el archivo package.xml del paquete (~/ros2_ws/src/ros2_line_follower_camera/package.xml) y asegúrate de incluir dependencias:

<exec_depend>rclpy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>cv_bridge</exec_depend>

Instala ros-humble-cv-bridge:

sudo apt install -y ros-humble-cv-bridge

2. Nodo Python line_follower_node.py

Crea el archivo ~/ros2_ws/src/ros2_line_follower_camera/ros2_line_follower_camera/line_follower_node.py:

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node

from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist

from cv_bridge import CvBridge
import cv2
import numpy as np


class LineFollowerNode(Node):
    def __init__(self):
        super().__init__('line_follower_node')

        # Parámetros configurables
        self.declare_parameter('camera_topic', '/camera/image_raw')
        self.declare_parameter('linear_speed', 0.15)   # m/s
        self.declare_parameter('kp_angular', 0.0025)   # ganancia proporcional
        self.declare_parameter('view_height_ratio', 0.3)  # zona inferior de la imagen a usar

        camera_topic = self.get_parameter('camera_topic').get_parameter_value().string_value
        self.linear_speed = self.get_parameter('linear_speed').get_parameter_value().double_value
        self.kp_angular = self.get_parameter('kp_angular').get_parameter_value().double_value
        self.view_height_ratio = self.get_parameter('view_height_ratio').get_parameter_value().double_value

        # Puente ROS 2 <-> OpenCV
        self.bridge = CvBridge()

        # Suscriptor a la cámara
        self.image_sub = self.create_subscription(
            Image,
            camera_topic,
            self.image_callback,
            10
        )

        # Publicador de velocidad
        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)

        self.get_logger().info(f'Line follower node iniciado. Suscrito a {camera_topic}')

    def image_callback(self, msg: Image):
        # Convertir a imagen OpenCV
        try:
            frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        except Exception as e:
            self.get_logger().error(f'Error al convertir imagen: {e}')
            return

        height, width, _ = frame.shape

        # Usar solo la franja inferior de la imagen para buscar la línea
        roi_height = int(height * self.view_height_ratio)
        y_start = height - roi_height
        roi = frame[y_start:height, 0:width]

        # Convertir a escala de grises
        gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)

        # Suavizar para reducir ruido
        blur = cv2.GaussianBlur(gray, (5, 5), 0)

        # Umbral binario: asumimos línea negra sobre fondo claro
        _, binary = cv2.threshold(blur, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)

        # Encontrar contornos
        contours, _ = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

        if contours:
            # Tomar el contorno más grande como la línea principal
            largest_contour = max(contours, key=cv2.contourArea)
            M = cv2.moments(largest_contour)

            if M['m00'] > 0:
                cx = int(M['m10'] / M['m00'])  # centroide x
                cy = int(M['m01'] / M['m00'])  # centroide y (en ROI)

                # Error de posición (positivo si la línea está a la derecha)
                error_x = cx - width // 2

                # Control proporcional para la velocidad angular
                angular_z = -self.kp_angular * float(error_x)

                # Crear mensaje Twist
                cmd = Twist()
                cmd.linear.x = self.linear_speed
                cmd.angular.z = angular_z

                self.cmd_vel_pub.publish(cmd)

                self.get_logger().debug(
                    f'Linea detectada: cx={cx}, error_x={error_x}, ang_z={angular_z:.3f}'
                )
            else:
                # No se pudo calcular el centroide: para el robot
                self.stop_robot('Moment zero area (m00=0)')

        else:
            # Sin contornos: línea no encontrada, parar robot
            self.stop_robot('No se encontraron contornos para la línea')

    def stop_robot(self, reason: str):
        cmd = Twist()
        cmd.linear.x = 0.0
        cmd.angular.z = 0.0
        self.cmd_vel_pub.publish(cmd)
        self.get_logger().warn(f'Robot detenido: {reason}')


def main(args=None):
    rclpy.init(args=args)
    node = LineFollowerNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.stop_robot('Nodo finalizado')
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Dale permisos de ejecución:

chmod +x ~/ros2_ws/src/ros2_line_follower_camera/ros2_line_follower_camera/line_follower_node.py

3. Ajustar setup.py del paquete

Edita ~/ros2_ws/src/ros2_line_follower_camera/setup.py:

from setuptools import setup

package_name = 'ros2_line_follower_camera'

setup(
    name=package_name,
    version='0.0.1',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
         ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='tu_nombre',
    maintainer_email='tu_email@example.com',
    description='Nodo seguidor de línea usando cámara OV5647 en Raspberry Pi 4 con ROS 2 Humble',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'line_follower_node = ros2_line_follower_camera.line_follower_node:main',
        ],
    },
)

---

Modelado del robot (URDF + ros2_control + diff_drive_controller)

Aunque el foco es el seguidor de línea, necesitamos que el UGV Beast se expose en ROS 2 como robot diferencial. Crearemos un URDF sencillo con diff_drive_controller.

1. Parámetros geométricos del UGV Beast

Debes medir tu robot:

• Radio de rueda (wheel_radius): por ejemplo, 0.03 m (ruedas de 6 cm de diámetro).
• Distancia entre centros de ruedas (wheel_separation o track width): por ejemplo, 0.18 m.

Añade estos valores a un archivo YAML de parámetros del controlador.

2. URDF simplificado

Crea un paquete de descripción ugv_beast_description:

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake ugv_beast_description

Dentro crea urdf/ugv_beast.urdf.xacro. Para no alargar excesivamente, indicamos los elementos clave que deben existir:

• base_link
• left_wheel_link y right_wheel_link
• Juntas left_wheel_joint y right_wheel_joint de tipo continuous
• Plugin ros2_control con hardware y diff_drive_system.

En este caso práctico básico asumiremos que el control bajo nivel (GPIO/PWM) está gestionado fuera de ros2_control (por ejemplo, por un nodo propio o por firmware de la placa del Beast). Lo importante es que el diff_drive_controller publique/consuma /cmd_vel y odom.

---

Compilación, instalación y ejecución

1. Compilar el workspace con colcon

cd ~/ros2_ws
colcon build

Una vez completado:

echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc

2. Nodo de la cámara

Dependiendo del driver utilizado, puedes exponer la cámara como un nodo ROS 2. Una opción ligera en Ubuntu es usar un nodo genérico tipo v4l2_camera (si lo instalas). Para mantener el caso práctico sencillo, puedes usar un nodo ejemplo como image_tools (incluido en ros-humble-desktop) adaptado, pero lo más realista es:

Instalar v4l2_camera:

sudo apt install -y ros-humble-v4l2-camera

Lanzar el nodo de cámara:

ros2 run v4l2_camera v4l2_camera_node --ros-args -p video_device:=/dev/video0 -p image_size:="[640,480]"

Este nodo publicará en /image_raw. Ajusta el parámetro de tu line_follower_node para usar ese tópico, o remapea.

Ejemplo ejecutando el seguidor de línea con remapeo:

ros2 run ros2_line_follower_camera line_follower_node \
  --ros-args -p camera_topic:=/image_raw

3. Lanzar el controlador diferencial (diff_drive_controller)

Asumiendo que tienes un ros2_control configurado y un archivo de parámetros YAML, lanzarías algo como:

ros2 launch ugv_beast_bringup ugv_beast_diff_drive.launch.py

En este caso práctico básico, si aún no tienes implementada la parte ros2_control real con hardware, puedes:

• Probar el nodo line_follower_node sin movimiento, sólo examinando /cmd_vel.
• Implementar posteriormente el puente /cmd_vel → GPIO/PWM según tu hardware.

---

Validación paso a paso

1. Verificar que la cámara funciona

1. Asegúrate de que /dev/video0 existe:

bash
ls /dev/video0

1. Lanza el nodo v4l2_camera:

bash
ros2 run v4l2_camera v4l2_camera_node --ros-args -p video_device:=/dev/video0

1. Comprueba que el tópico de imagen está activo:

bash
ros2 topic list

Debes ver algo como:

• /image_raw

• /camera_info

• Comprueba el tipo de mensaje:

bash
ros2 topic echo /image_raw --qos-reliability best_effort --qos-durability volatile

Verás muchos datos binarios/imprimibles. Para medir tasa de publicación:

bash
ros2 topic hz /image_raw

Criterio de éxito: ≥ 15 Hz.

2. Verificar el nodo ros2-line-follower-camera

1. Abre otra terminal (con source ~/ros2_ws/install/setup.bash).
2. Lanza el nodo:

bash
ros2 run ros2_line_follower_camera line_follower_node \
--ros-args -p camera_topic:=/image_raw

1. Comprueba que se crea el tópico /cmd_vel:

bash
ros2 topic list | grep cmd_vel

1. Mide la frecuencia de publicación de /cmd_vel:

bash
ros2 topic hz /cmd_vel

Criterio de éxito: ≥ 10 Hz mientras haya imágenes entrantes.

1. Coloca delante de la cámara una superficie blanca con una línea negra (cinta aislante). Asegúrate de que la línea cruza la parte inferior del campo de visión de la cámara.

2. Cuando la línea esté centrada, espera ver que:

3. linear.x ≈ valor del parámetro linear_speed (0.15).

4. angular.z ≈ 0.

5. Desplaza la línea hacia la izquierda/derecha; deberías observar:

6. Si la línea se mueve hacia la derecha en la imagen (error positivo), angular.z será negativo (gira hacia la derecha, según la convención de tu robot).

7. Si la línea se mueve hacia la izquierda, angular.z será positivo.

Puedes comprobarlo con:

bash
ros2 topic echo /cmd_vel

3. Validar en el UGV Beast en movimiento

1. Coloca el UGV Beast en el suelo sobre una pista con línea negra sobre blanco (recta de al menos 2–3 m).
2. Lanza los nodos:

3. Cámara:

   bash
ros2 run v4l2_camera v4l2_camera_node --ros-args -p video_device:=/dev/video0 -p image_size:="[640,480]"

4. Seguidor de línea:

   bash
ros2 run ros2_line_follower_camera line_follower_node \
--ros-args -p camera_topic:=/image_raw -p linear_speed:=0.10 -p kp_angular:=0.003

5. Si el puente cmd_vel → motores está funcional (por diff_drive_controller o nodo propio), el robot empezará a moverse hacia adelante y ajustar su dirección.

6. Criterios de validación medibles:

7. El robot no se sale de la línea más de 5 cm lateralmente en la mayoría del recorrido.
8. En curvas suaves, el robot desacelera el giro adecuadamente y no “zigzaguea” de manera extrema.
9. El movimiento responde en menos de 0.3 s al mover la línea manualmente.

---

Troubleshooting (errores típicos y soluciones)

1. No aparece /dev/video0

Síntoma: ls /dev/video* no muestra /dev/video0.

Causas y soluciones:
– La cámara no está bien conectada:
– Apaga la Pi, revisa el cable plano FPC, verifica orientación y que la pestaña CSI está bien fijada.
– Configuración en config.txt incorrecta:
– Asegúrate de tener:

```text
start_x=1
gpu_mem=128
dtoverlay=ov5647   # o el recomendado por Arducam para OV5647
```

• Reinicia después de modificarlo.
• Firmware/overlay no soporta esa cámara:
• Revisa la documentación de Arducam 5MP OV5647 para Ubuntu 22.04 en Raspberry Pi 4; puede requerir un overlay distinto o actualización de firmware.

2. line_follower_node falla al convertir la imagen

Síntoma: En la consola, mensajes: Error al convertir imagen.

Causas y soluciones:
– Tipo de codificación no soportado:
– Asegúrate de que el nodo de cámara publica en un formato compatible (bgr8 o convertible).
– Con ros2 topic echo /image_raw -n 1, revisa el campo encoding.
– Si es yuyv o similar, modifica:

```python
frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
```

por

```python
frame = self.bridge.imgmsg_to_cv2(msg)
```

y prueba.

3. Proceso de detección no encuentra contornos

Síntoma: El robot se mantiene parado con alertas Robot detenido: No se encontraron contornos para la línea.

Causas y soluciones:
– Contraste insuficiente entre línea y fondo:
– Usa fondo blanco brillante (cartulina) y cinta negra muy oscura.
– Aumenta iluminación.
– Umbral inadecuado:
– En lugar de umbral automático OTSU, prueba con un valor fijo:

```python
_, binary = cv2.threshold(blur, 100, 255, cv2.THRESH_BINARY_INV)
```

• Ajusta 100 a tu entorno.
• La línea está fuera de la región de interés:
• Cambia view_height_ratio (por ejemplo, 0.5 para usar media imagen).

4. El robot oscila mucho (zigzag)

Síntoma: El UGV empeora la trayectoria, corrigiendo en exceso la dirección.

Causas y soluciones:
– Ganancia kp_angular demasiado alta:
– Reduce el valor: por ejemplo, de 0.003 a 0.0015.
– Velocidad lineal demasiado alta:
– Baja linear_speed a 0.08 o 0.05 m/s para principiantes.
– Retardo elevado de cámara:
– Reduce resolución (image_size:="[320,240]") para aumentar FPS.

5. Tópico /cmd_vel no llega al controlador de motores

Síntoma: Ves datos en /cmd_vel pero las ruedas no se mueven.

Causas y soluciones:
– diff_drive_controller no está configurado:
– Asegúrate de haber creado y lanzado un controller_manager con diff_drive_controller.
– Puente hardware no implementado:
– Este caso práctico no entra al detalle de GPIO/PWM; necesitás un nodo que subscribe /cmd_vel y maneje los pines o que tu placa Beast haga de interfaz.
– Espacio de nombres (namespace) diferente:
– Verifica que tu controlador espera /cmd_vel y no otro tópico como /diff_drive_controller/cmd_vel_unstamped.

6. ros2 topic hz /image_raw muestra tasas muy bajas (≤ 5 Hz)

Síntoma: La cámara publica a muy baja frecuencia, el robot reacciona tarde.

Causas y soluciones:
– Resolución demasiado alta:
– Reduce image_size a [320,240] en v4l2_camera.
– Exceso de carga de CPU:
– Verifica con top que no haya otros procesos pesados.
– Desactiva nodos innecesarios durante pruebas.

7. Error al construir el workspace con colcon

Síntoma: colcon build falla en ros2_line_follower_camera con errores de dependencias.

Causas y soluciones:
– Falta alguna dependencia en package.xml:
– Asegúrate de incluir rclpy, sensor_msgs, geometry_msgs, cv_bridge.
– No instalaste ros-humble-cv-bridge o python3-opencv:
– Instálalos con:

```bash
sudo apt install -y ros-humble-cv-bridge python3-opencv
```

• No ejecutaste source /opt/ros/humble/setup.bash antes de compilar:
• Asegúrate de que lo tienes en el .bashrc.

---

Mejoras y variantes

Una vez tengas el caso básico funcionando, puedes plantear las siguientes mejoras:

1. Control más avanzado (PID)
   En lugar de sólo control proporcional (kp_angular), añade términos integral y derivativo para suavizar y anticipar correcciones.

2. Cálculo de la línea como recta
   En vez de usar solo el centroide del contorno, usa técnicas como:

3. Ajuste de línea (regresión) a partir de puntos de la línea.

4. Hough transform para detectar segmentos.

5. Seguimiento de intersecciones
   Extiende el algoritmo para reconocer cruces en T o intersecciones y tomar decisiones:

6. Girar a la izquierda o derecha según bandera.

7. Parar en un cruce y esperar orden externa (ej. topic de alto nivel).

8. Integrar con Nav2 y SLAM
   Aunque aquí sólo usamos la línea, tu UGV Beast puede:

9. Mapear el entorno con slam_toolbox.
10. Usar nav2 para navegación global cuando no haya línea.

11. Combinar modos “seguimiento de línea” y “navegación libre”.

12. Publicar imágenes procesadas
    Crea un tópico /line_follower/debug_image para ver por RViz o herramientas de visualización la binarización y contornos, ayudando a depurar.

13. Parámetros dinámicos
    Usa parámetros ROS 2 dinámicos para ajustar kp_angular, linear_speed y view_height_ratio en tiempo real sin reiniciar el nodo.

---

Checklist de verificación (para el alumno)

Marca cada ítem al completarlo:

• [ ] Ubuntu Server 22.04 64‑bit instalado y actualizado en Raspberry Pi 4 Model B 4GB.
• [ ] ROS 2 Humble y paquetes (ros-humble-desktop, ros-humble-ros2-control, ros-humble-diff-drive-controller, ros-humble-robot-localization, ros-humble-slam-toolbox, ros-humble-nav2-*, ros-humble-rviz2) instalados con apt.
• [ ] Cámara Arducam 5MP OV5647 Camera Module conectada al puerto CSI y habilitada en /boot/firmware/config.txt.
• [ ] /dev/video0 visible tras el arranque de la Raspberry Pi.
• [ ] Nodo v4l2_camera funcionando y publicando en /image_raw a ≥ 15 Hz (ros2 topic hz /image_raw).
• [ ] Workspace ~/ros2_ws creado, con paquete ros2_line_follower_camera y dependencias instaladas (cv_bridge, python3-opencv).
• [ ] colcon build se ejecuta sin errores y source ~/ros2_ws/install/setup.bash está en .bashrc.
• [ ] Nodo line_follower_node se ejecuta correctamente y crea el tópico /cmd_vel.
• [ ] Con la línea negra en el campo de visión, /cmd_vel publica valores razonables de linear.x y angular.z.
• [ ] El puente /cmd_vel → controlador de motores del UGV Beast funciona y las ruedas reaccionan.
• [ ] En una pista de al menos 5 m, el UGV Beast sigue la línea sin desviarse más de ±5 cm la mayor parte del tiempo.

Si todos los ítems están marcados, has completado con éxito el caso práctico ros2-line-follower-camera para el UGV Beast (ROS 2) – Raspberry Pi 4 Model B 4GB + Arducam 5MP OV5647 Camera Module a nivel básico.