ROS2系列教程：MicroROS接入（原理+仿真）

1
┌─────────────────── 上位机（Linux） ───────────────────┐
2
│  ROS2 图：/micro_ros_node 发布 /cmd_vel 订阅 /odom     │
3
│              │                                        │
4
│      micro-ROS Agent（代理，把串口数据转成 ROS2 DDS）    │
5
└──────────────│────────────────────────────────────────┘
6
               │ 串口 / USB / Wi-Fi（UART/Serial/UDP）
7
┌──────────────│────────────────────────────────────────┐
8
│               │                                        │
9
│      micro-ROS Client（运行在 MCU 上的 rcl 实现）        │
10
│   MCU：STM32 / ESP32 / Arduino ...                    │
11
└───────────────────────────────────────────────────────┘

1
rclc_support_init      初始化运行时上下文
2
    ↓
3
rclc_node_init_default 创建节点
4
    ↓
5
rclc_publisher_init_*  创建发布者（或订阅者/服务）
6
    ↓
7
rclc_executor_init     创建执行器（绑定节点）
8
    ↓
9
rclc_executor_add_*    注册订阅者回调等
10
    ↓
11
rclc_executor_spin     循环执行（主循环）

1
// heartbeat_pub.c：MicroROS 心跳发布者（模拟 MCU 节点）
2
#include <rcl/rcl.h>
3
#include <rclc/rclc.h>
4
#include <rclc/executor.h>
5
#include <std_msgs/msg/int32.h>
6

7
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; \
8
  if ((temp_rc != RCL_RET_OK)) { for (;;) {} } }
9
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; \
10
  if ((temp_rc != RCL_RET_OK)) {} }
11

12
rcl_publisher_t publisher;
13
std_msgs__msg__Int32 msg;
14

15
// 定时回调：心跳值加 1
16
void timer_callback(rcl_timer_t *timer, int64_t last_call_time) {
17
  (void) last_call_time;
18
  if (timer != NULL) {
19
    msg.data++;
20
    rcl_publish(&publisher, &msg, NULL);
21
    printf("heartbeat: %d\n", msg.data);
22
  }
23
}
24

25
int main() {
26
  rcl_allocator_t allocator = rcl_get_default_allocator();
27
  rclc_support_t support;
28
  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
29
  RCCHECK(rcl_init_options_init(&init_options, rcl_get_default_allocator()));
30
  RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
31

32
  // 创建节点
33
  rcl_node_t node;
34
  RCCHECK(rclc_node_init_default(&node, "heartbeat_node", "", &support));
35

36
  // 创建发布者
37
  RCCHECK(rclc_publisher_init_default(
38
    &publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "/heartbeat"));
39

40
  // 创建定时器（500ms 触发一次）
41
  rcl_timer_t timer;
42
  RCCHECK(rclc_timer_init_default(
43
    &timer, &support, RCL_MS_TO_NS(500), timer_callback));
44

45
  // 创建执行器并注册定时器
46
  rclc_executor_t executor;
47
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
48
  RCCHECK(rclc_executor_add_timer(&executor, &timer));
49

50
  printf("heartbeat_node started, publishing to /heartbeat\n");
51

52
  // 主循环
53
  for (;;) {
54
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
55
  }
56

57
  RCCHECK(rcl_publisher_fini(&publisher, &node));
58
  RCCHECK(rcl_node_fini(&node));
59
  return 0;
60
}

1
// cmd_sub.c：MicroROS 订阅者（接收 /cmd_vel 指令，模拟执行）
2
#include <rcl/rcl.h>
3
#include <rclc/rclc.h>
4
#include <rclc/executor.h>
5
#include <geometry_msgs/msg/twist.h>
6

7
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; \
8
  if ((temp_rc != RCL_RET_OK)) { for (;;) {} } }
9

10
rcl_subscription_t subscriber;
11
geometry_msgs__msg__Twist msg;
12

13
// 订阅回调：收到速度指令后"执行"（这里打印）
14
void subscription_callback(const void *msgin) {
15
  const geometry_msgs__msg__Twist *twist = msgin;
16
  printf("exec cmd_vel: linear=%.2f angular=%.2f\n",
17
         twist->linear.x, twist->angular.z);
18
}
19

20
int main() {
21
  rcl_allocator_t allocator = rcl_get_default_allocator();
22
  rclc_support_t support;
23
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
24

25
  rcl_node_t node;
26
  RCCHECK(rclc_node_init_default(&node, "cmd_sub_node", "", &support));
27

28
  RCCHECK(rclc_subscription_init_default(
29
    &subscriber, &node,
30
    ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
31
    "/cmd_vel"));
32

33
  rclc_executor_t executor;
34
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
35
  RCCHECK(rclc_executor_add_subscription(
36
    &executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
37

38
  printf("cmd_sub_node started, waiting for /cmd_vel...\n");
39

40
  for (;;) {
41
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
42
  }
43

44
  RCCHECK(rcl_subscription_fini(&subscriber, &node));
45
  RCCHECK(rcl_node_fini(&node));
46
  return 0;
47
}

1
// led_srv.c：MicroROS 服务端（设置 LED 状态的服务）
2
#include <rcl/rcl.h>
3
#include <rclc/rclc.h>
4
#include <rclc/executor.h>
5
#include <example_interfaces/srv/set_bool.h>
6

7
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; \
8
  if ((temp_rc != RCL_RET_OK)) { for (;;) {} } }
9

10
rcl_service_t service;
11
example_interfaces__srv__SetBool_Request req;
12
example_interfaces__srv__SetBool_Response res;
13

14
// 服务回调：处理"设置 LED"请求
15
void service_callback(const void *request, void *response) {
16
  const example_interfaces__srv__SetBool_Request *r = request;
17
  example_interfaces__srv__SetBool_Response *s = response;
18
  s->success = true;
19
  if (r->data) {
20
    s->message = "led on";   // 真实 MCU 这里调用 HAL_GPIO_WritePin
21
  } else {
22
    s->message = "led off";
23
  }
24
  printf("set led: %s\n", s->message);
25
}
26

27
int main() {
28
  rcl_allocator_t allocator = rcl_get_default_allocator();
29
  rclc_support_t support;
30
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
31

32
  rcl_node_t node;
33
  RCCHECK(rclc_node_init_default(&node, "led_srv_node", "", &support));
34

35
  RCCHECK(rclc_service_init_default(
36
    &service, &node,
37
    ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, SetBool),
38
    "/set_led"));
39

40
  rclc_executor_t executor;
41
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
42
  RCCHECK(rclc_executor_add_service(
43
    &executor, &service, &req, &res, service_callback));
44

45
  printf("led_srv_node started, service /set_led ready\n");
46

47
  for (;;) {
48
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
49
  }
50
  return 0;
51
}