Tianci/张天赐

Tianci/张天赐

清华大学博后在读

micro_ros下位机自动连接micro_ros_agent

Published: 2 minute read, with 575 words. Post views:

参考：官方文档

测试环境：

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

0. 写在前面

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

1. micro_ros_reconnection

1.1 官方示例

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)
  {
    (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

  • 重复插拔几次串口，观察是否自动连接；

  • 当然是自动连接了 :)

Tags: , ,

Categories:

Comments

Recent posts