📚 课程目录第2章 LED闪烁控制

2. LED闪烁控制

任务目标

本案例将实现一个基于micro-ROS的LED闪烁控制器：开发板通过WiFi接入ROS 2网络，订阅/led_cmd话题。话题消息为std_msgs/msg/Int32，数值代表LED闪烁周期（毫秒）。收到消息后，开发板以该周期驱动LED闪烁；未收到消息时，LED保持熄灭。

硬件与软件准备

硬件：ESP32开发板、板载LED（GPIO2）、USB数据线
软件：Ubuntu 22.04 + ROS 2 Humble、micro_ROS_Agent、Arduino IDE（安装esp32核心与micro_ROS_Arduino库）
网络：开发板与PC处于同一局域网

完整代码（Arduino Sketch）

#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>

#define LED_PIN 2
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){ return false; } }
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){ } }

rcl_publisher_t publisher;
rcl_subscription_t subscriber;
std_msgs__msg__Int32 msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

int blink_interval = 0;  // 0表示LED熄灭
unsigned long last_toggle = 0;
bool led_state = false;

void subscription_callback(const void *msgin) {
  const std_msgs__msg__Int32 *in_msg = (const std_msgs__msg__Int32 *)msgin;
  blink_interval = in_msg->data;
  if (blink_interval < 0) blink_interval = 0;
}

bool create_entities() {
  allocator = rcl_get_default_allocator();
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  RCCHECK(rclc_node_init_default(&node, "led_blink_node", "", &support));
  RCCHECK(rclc_subscription_init_default(
    &subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "/led_cmd"));
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
  return true;
}

void destroy_entities() {
  rcl_subscription_fini(&subscriber, &node);
  rclc_executor_fini(&executor);
  rclc_node_fini(&node);
  rclc_support_fini(&support);
}

void setup() {
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);
  Serial.begin(115200);
  set_microros_wifi_transports("YOUR_SSID", "YOUR_PASSWORD", "192.168.1.100", 8888);
  delay(2000);
  create_entities();
}

void loop() {
  rclc_executor_spin_some(&executor, 10);
  
  if (blink_interval > 0) {
    unsigned long now = millis();
    if (now - last_toggle >= (unsigned long)blink_interval) {
      last_toggle = now;
      led_state = !led_state;
      digitalWrite(LED_PIN, led_state ? HIGH : LOW);
    }
  } else {
    digitalWrite(LED_PIN, LOW);
    led_state = false;
  }
  
  if (rmw_uros_epoch_synchronized() == false) {
    // 与Agent连接断开时自动重连
    if (WiFi.status() != WL_CONNECTED) {
      delay(100);
    } else if (create_entities() == false) {
      delay(1000);
      destroy_entities();
    }
  }
}

运行步骤

修改WiFi配置：将代码中YOUR_SSID、YOUR_PASSWORD替换为实际WiFi信息，192.168.1.100替换为运行micro_ROS_Agent的PC的IP地址。

启动micro_ROS_Agent（PC终端）：

source /opt/ros/humble/setup.bash
ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888

编译烧录：在Arduino IDE中选择ESP32开发板型号和对应串口，上传代码。打开串口监视器（115200波特率），等待ESP32连接Agent。

验证连接（PC新终端）：

source /opt/ros/humble/setup.bash
ros2 node list   # 应看到 /led_blink_node
ros2 topic list  # 应看到 /led_cmd

验证方法

在PC终端发布不同周期值，观察LED闪烁变化：

# LED以500ms周期闪烁
ros2 topic pub --once /led_cmd std_msgs/msg/Int32 "{data: 500}"

# LED以100ms周期快速闪烁
ros2 topic pub --once /led_cmd std_msgs/msg/Int32 "{data: 100}"

# 发送0或负数，LED熄灭
ros2 topic pub --once /led_cmd std_msgs/msg/Int32 "{data: 0}"

通过条件：

ros2 node list能发现/led_blink_node节点；
发布500后LED约每秒闪烁2次（亮灭各500ms）；
发布100后LED明显加速闪烁；
发布0后LED立即熄灭并保持。

常见问题

节点未出现在列表中：检查Agent终端是否有连接日志；确认ESP32与PC在同一网段；尝试关闭PC防火墙。
发布话题无反应：确认话题名完全匹配/led_cmd，消息类型为std_msgs/msg/Int32。
WiFi连接失败：ESP32仅支持2.4GHz频段，确认路由器未开启5GHz隔离。