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 
#include 
#include 
#include 
#include 
#include 
#include 

#include "M5Atom.h"

#include   //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 
  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)
  {
    (void) last_call_time;
    if (timer != NULL) {
      rcl_publish(&publisher, &msg, NULL);
      static int cnt = 0;
      msg.linear.x = 0.2;                            //const linear.x
      msg.angular.z = 1.0 - 0.001 * cnt;             //variable angular.z
      cnt++;
    }
  }

  bool create_entities()
  {
    allocator = rcl_get_default_allocator();

    // create init_options
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

    // create node
    RCCHECK(rclc_node_init_default(&node, "int32_publisher_rclc", "", &support));

    // create publisher
    RCCHECK(rclc_publisher_init_best_effort(
              &publisher,
              &node,
              ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
              "turtle1/cmd_vel"));

    // create timer,
    const unsigned int timer_timeout = 1000;
    RCCHECK(rclc_timer_init_default(
              &timer,
              &support,
              RCL_MS_TO_NS(timer_timeout),
              timer_callback));

    // create executor
    executor = rclc_executor_get_zero_initialized_executor();
    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
    RCCHECK(rclc_executor_add_timer(&executor, &timer));

    return true;
  }

  void destroy_entities()
  {
    rmw_context_t * rmw_context = rcl_context_get_rmw_context(&support.context);
    (void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);

    rcl_publisher_fini(&publisher, &node);
    rcl_timer_fini(&timer);
    rclc_executor_fini(&executor);
    rcl_node_fini(&node);
    rclc_support_fini(&support);
  }

  void setup() {
    M5.begin(true, false, true);
    M5.dis.drawpix(0, 0x00ff00);
    set_microros_transports();
    state = WAITING_AGENT;

    // changed!-->msg initialization
    msg.linear.x = 0;
    msg.linear.y = 0;
    msg.linear.z = 0;
    msg.angular.x = 0;
    msg.angular.y = 0;
    msg.angular.z = 0;
  }

  void loop() {
    switch (state) {
      case WAITING_AGENT:
        EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;);
        break;
      case AGENT_AVAILABLE:
        state = (true == create_entities()) ? AGENT_CONNECTED : WAITING_AGENT;
        if (state == WAITING_AGENT) {
          destroy_entities();
        };
        break;
      case AGENT_CONNECTED:
        EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;);
        if (state == AGENT_CONNECTED) {
          rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
        }
        break;
      case AGENT_DISCONNECTED:
        destroy_entities();
        state = WAITING_AGENT;
        break;
      default:
        break;
    }

    if (state == AGENT_CONNECTED) {
      M5.dis.drawpix(0, 0x00ff00);  // 绿色
    }
    else if (state == WAITING_AGENT) {
      M5.dis.drawpix(0, 0xfff000);  // 黄色
    }
    else if (state == AGENT_DISCONNECTED) {
      M5.dis.drawpix(0, 0xff0000);  // 红色
    }
    else if (state == AGENT_AVAILABLE) {
      M5.dis.drawpix(0, 0x0000f0);  //蓝色
    }
  }

1.3 代码解析

• 关于发布"turtle1/cmd_vel"的部分代码见前文，不再赘述
• micro_ros通讯状态定义

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

• 示例代码中定义了一个类似软件定时器的宏

#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)\

• 在loop循环中，每隔一定时间，检查通讯状态

switch (state) {
      case WAITING_AGENT:
        EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;);
        break;
      case AGENT_AVAILABLE:
        state = (true == create_entities()) ? AGENT_CONNECTED : WAITING_AGENT;
        if (state == WAITING_AGENT) {
          destroy_entities();
        };
        break;
      case AGENT_CONNECTED:
        EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;);
        if (state == AGENT_CONNECTED) {
          rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
        }
        break;
      case AGENT_DISCONNECTED:
        destroy_entities();
        state = WAITING_AGENT;
        break;
      default:
        break;
    }

• 引入M5.ATOM.h,方便控制彩色LED来显示不同的micro_ros通讯状态

	#include "M5Atom.h"
    M5.begin(true, false, true);
    M5.dis.drawpix(0, 0x00ff00);
    if (state == AGENT_CONNECTED) {
      M5.dis.drawpix(0, 0x00ff00);  // 绿色
    }
    else if (state == WAITING_AGENT) {
      M5.dis.drawpix(0, 0xfff000);  // 黄色
    }
    else if (state == AGENT_DISCONNECTED) {
      M5.dis.drawpix(0, 0xff0000);  // 红色
    }
    else if (state == AGENT_AVAILABLE) {
      M5.dis.drawpix(0, 0x0000f0);  //蓝色
    }

• 关于LED的代码部分，可根据自己的led来改写，也可删除。

2. 测试

• 编译代码，然后上传
• 首先提升串口读写权限(确保自己的串口是ttyUSB0，因硬件而异)

  sudo chmod -R 777 /dev/ttyUSB0
  

• 开启micro_agent

  ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0
  

• 此时，下位机连接上位机，无需重启便自动连接micro_ros_agent
• 重复插拔几次串口，观察是否自动连接；
• 当然是自动连接了