Caso práctico: Waypoints GPS en UGV Beast (ROS 2)

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.