micro-ROS con ESP32: tu primer nodo de ROS 2

// Publicador de micro-ROS con reconexión automática: publica un entero que sube de uno en uno en /esp32/contador.
// Si el agente se para o se reinicia, la placa lo detecta y vuelve a conectar sola cuando el agente reaparece.
#include <Arduino.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rmw_microros/rmw_microros.h>
#include <std_msgs/msg/int32.h>

rcl_publisher_t publicador;
std_msgs__msg__Int32 mensaje;
rclc_executor_t ejecutor;
rclc_support_t soporte;
rcl_allocator_t asignador;
rcl_node_t nodo;
rcl_timer_t temporizador;

enum Estado { ESPERANDO_AGENTE, AGENTE_DISPONIBLE, AGENTE_CONECTADO, AGENTE_PERDIDO };
Estado estado = ESPERANDO_AGENTE;

#define CADA_MS(MS, ACCION) do { static uint32_t ultima = 0; if (millis() - ultima >= (MS)) { ultima = millis(); ACCION; } } while (0)

void al_temporizador(rcl_timer_t *t, int64_t) {
  if (t != NULL) {
    rcl_publish(&publicador, &mensaje, NULL);
    mensaje.data++;
  }
}

bool crear_entidades() {
  asignador = rcl_get_default_allocator();
  if (rclc_support_init(&soporte, 0, NULL, &asignador) != RCL_RET_OK) return false;
  if (rclc_node_init_default(&nodo, "esp32_nodo", "", &soporte) != RCL_RET_OK) return false;
  if (rclc_publisher_init_default(&publicador, &nodo,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "esp32/contador") != RCL_RET_OK) return false;
  if (rclc_timer_init_default(&temporizador, &soporte, RCL_MS_TO_NS(1000), al_temporizador) != RCL_RET_OK) return false;
  if (rclc_executor_init(&ejecutor, &soporte.context, 1, &asignador) != RCL_RET_OK) return false;
  if (rclc_executor_add_timer(&ejecutor, &temporizador) != RCL_RET_OK) return false;
  return true;
}

void destruir_entidades() {
  rmw_context_t *contexto = rcl_context_get_rmw_context(&soporte.context);
  (void)rmw_uros_set_context_entity_destroy_session_timeout(contexto, 0);
  rcl_publisher_fini(&publicador, &nodo);
  rcl_timer_fini(&temporizador);
  rclc_executor_fini(&ejecutor);
  rcl_node_fini(&nodo);
  rclc_support_fini(&soporte);
}

void setup() {
  Serial.begin(115200);
  set_microros_serial_transports(Serial);
  mensaje.data = 0;
}

void loop() {
  switch (estado) {
    case ESPERANDO_AGENTE:
      CADA_MS(500, estado = (rmw_uros_ping_agent(100, 1) == RMW_RET_OK) ? AGENTE_DISPONIBLE : ESPERANDO_AGENTE);
      break;
    case AGENTE_DISPONIBLE:
      estado = crear_entidades() ? AGENTE_CONECTADO : ESPERANDO_AGENTE;
      if (estado == ESPERANDO_AGENTE) destruir_entidades();
      break;
    case AGENTE_CONECTADO:
      CADA_MS(200, estado = (rmw_uros_ping_agent(100, 1) == RMW_RET_OK) ? AGENTE_CONECTADO : AGENTE_PERDIDO);
      if (estado == AGENTE_CONECTADO) rclc_executor_spin_some(&ejecutor, RCL_MS_TO_NS(100));
      break;
    case AGENTE_PERDIDO:
      destruir_entidades();
      estado = ESPERANDO_AGENTE;
      break;
  }
}

// Suscriptor de micro-ROS con reconexión automática: escucha /esp32/led (std_msgs/Bool) y enciende o apaga el LED del pin 17.
// Montaje: pin 17 -> resistencia de 220 ohm -> pata larga (ánodo) del LED -> pata corta (cátodo) a GND.
// Si el agente se para o se reinicia, la placa lo detecta, apaga el LED y vuelve a conectar sola cuando el agente reaparece.
#include <Arduino.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rmw_microros/rmw_microros.h>
#include <std_msgs/msg/bool.h>

#define PIN_LED 17

rcl_subscription_t suscriptor;
std_msgs__msg__Bool mensaje;
rclc_executor_t ejecutor;
rclc_support_t soporte;
rcl_allocator_t asignador;
rcl_node_t nodo;

enum Estado { ESPERANDO_AGENTE, AGENTE_DISPONIBLE, AGENTE_CONECTADO, AGENTE_PERDIDO };
Estado estado = ESPERANDO_AGENTE;

// Ejecuta la acción como mucho una vez cada MS milisegundos.
#define CADA_MS(MS, ACCION) do { static uint32_t ultima = 0; if (millis() - ultima >= (MS)) { ultima = millis(); ACCION; } } while (0)

void al_recibir(const void *entrada) {
  const std_msgs__msg__Bool *m = (const std_msgs__msg__Bool *)entrada;
  digitalWrite(PIN_LED, m->data ? HIGH : LOW);
}

bool crear_entidades() {
  asignador = rcl_get_default_allocator();
  if (rclc_support_init(&soporte, 0, NULL, &asignador) != RCL_RET_OK) return false;
  if (rclc_node_init_default(&nodo, "esp32_led", "", &soporte) != RCL_RET_OK) return false;
  if (rclc_subscription_init_default(&suscriptor, &nodo,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), "esp32/led") != RCL_RET_OK) return false;
  if (rclc_executor_init(&ejecutor, &soporte.context, 1, &asignador) != RCL_RET_OK) return false;
  if (rclc_executor_add_subscription(&ejecutor, &suscriptor, &mensaje, &al_recibir, ON_NEW_DATA) != RCL_RET_OK) return false;
  return true;
}

void destruir_entidades() {
  rmw_context_t *contexto = rcl_context_get_rmw_context(&soporte.context);
  (void)rmw_uros_set_context_entity_destroy_session_timeout(contexto, 0);  // no esperar a un agente que ya no está
  rcl_subscription_fini(&suscriptor, &nodo);
  rclc_executor_fini(&ejecutor);
  rcl_node_fini(&nodo);
  rclc_support_fini(&soporte);
}

void setup() {
  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED, LOW);
  Serial.begin(115200);
  set_microros_serial_transports(Serial);
}

void loop() {
  switch (estado) {
    case ESPERANDO_AGENTE:
      CADA_MS(500, estado = (rmw_uros_ping_agent(100, 1) == RMW_RET_OK) ? AGENTE_DISPONIBLE : ESPERANDO_AGENTE);
      break;
    case AGENTE_DISPONIBLE:
      estado = crear_entidades() ? AGENTE_CONECTADO : ESPERANDO_AGENTE;
      if (estado == ESPERANDO_AGENTE) destruir_entidades();
      break;
    case AGENTE_CONECTADO:
      CADA_MS(200, estado = (rmw_uros_ping_agent(100, 1) == RMW_RET_OK) ? AGENTE_CONECTADO : AGENTE_PERDIDO);
      if (estado == AGENTE_CONECTADO) rclc_executor_spin_some(&ejecutor, RCL_MS_TO_NS(100));
      break;
    case AGENTE_PERDIDO:
      digitalWrite(PIN_LED, LOW);  // seguridad: sin agente, el LED se apaga
      destruir_entidades();
      estado = ESPERANDO_AGENTE;
      break;
  }
}