micro_ros下位机自动连接micro_ros_agent

参考：官方文档
micro_ros_blog

测试环境：

• M5 Atom Lite (esp32-pico-d4 core)
• 旭日x3派(2G)+ros2 foxy

0. 写在前面

• 在micro_ros与上位机连接时，前文大部分情况下需要手动复位下位机
• 实际使用中，我们希望上电自动连接
• 尽管可以用软件控制复位，但仍较为麻烦
• 所以，我们希望能够有一种机制帮助我们实现自动连接micro_ros_agent

1. micro_ros_reconnection

1.1 官方示例

• 在micro_ros_arduino的代码中提供了一个micro-ros_reconnection.ino的示例
• 该代码利用状态机的方式判断上位机的agent是否开启，下位机是否就绪以及上位机与下位机是否连接等；
• 本文基于示例代码，通过下位机发布twist消息控制turtlesim的运动

1.2 完整代码

#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rmw_microros/rmw_microros.h>

#include "M5Atom.h"

#include <geometry_msgs/msg/twist.h>  //changed!

#define RCCHECK(fn) {
      rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){
     return false;}}
#define EXECUTE_EVERY_N_MS(MS, X)  do {
      \
    static volatile int64_t init = -1; \
    if (init == -1) {
      init = uxr_millis();} \
    if (uxr_millis() - init > MS) {
      X; init = uxr_millis();} \
  } while (0)\

  rclc_support_t support;
  rcl_node_t node;
  rcl_timer_t timer;
  rclc_executor_t executor;
  rcl_allocator_t allocator;
  rcl_publisher_t publisher;
  geometry_msgs__msg__Twist msg; //changed!-->modify msg type <twist__struct.h>
  bool micro_ros_init_successful;

  enum states {
   
    WAITING_AGENT,
    AGENT_AVAILABLE,
    AGENT_CONNECTED,
    AGENT_DISCONNECTED
  } state;

  void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
  {
   
    (