Caso práctico: teleoperación UGV Beast (ROS 2) con joystick

Caso práctico: teleoperación UGV Beast (ROS 2) con joystick — hero

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:

Función MCP2515 Pin en Raspberry Pi 4 Notas
VCC (3.3V) Pin 1 (3V3) Alimentación lógica
GND Pin 6 (GND) Tierra común
SPI_MOSI Pin 19 (GPIO10) MOSI (SPI0_MOSI)
SPI_MISO Pin 21 (GPIO9) MISO (SPI0_MISO)
SPI_SCK Pin 23 (GPIO11) SCLK (SPI0_SCLK)
SPI_CS Pin 24 (GPIO8) CE0 (SPI0_CE0_N), asignado a mcp2515
INT (interrup.) Pin 22 (GPIO25) Línea de interrupción del MCP2515

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).

Encuentra este producto y/o libros sobre este tema en Amazon

Ir a Amazon

Como afiliado de Amazon, gano con las compras que cumplan los requisitos. Si compras a través de este enlace, ayudas a mantener este proyecto.

Quiz rápido

Pregunta 1: ¿Qué plataforma se utilizará para construir el UGV Beast?




Pregunta 2: ¿Cuál es el propósito del HAT CAN MCP2515 en el proyecto?




Pregunta 3: ¿Qué herramienta se usará para teleoperar el UGV Beast?




Pregunta 4: ¿Cuál es la distancia mínima de frenado que se debe verificar?




Pregunta 5: ¿Qué velocidad lineal máxima debe responder el hardware de locomoción?




Pregunta 6: ¿Qué frecuencia de comandos cmd_vel se espera lograr?




Pregunta 7: ¿Qué tipo de maniobras se entrenarán con el UGV Beast?




Pregunta 8: ¿Cuál es el objetivo principal de generar trayectorias de referencia?




Pregunta 9: ¿Qué porcentaje de uso de CPU se espera mantener en la Raspberry Pi?




Pregunta 10: ¿Cuál es la latencia máxima permitida entre el joystick y las ruedas?




Carlos Núñez Zorrilla
Carlos Núñez Zorrilla
Electronics & Computer Engineer

Ingeniero Superior en Electrónica de Telecomunicaciones e Ingeniero en Informática (titulaciones oficiales en España).

Sígueme:

Related Posts

Scroll to Top