microros-agent会话频繁断连，销毁重建

Mostima

问题描述：

刚拿到四驱版fishbot时下载官方固件并使用WIFI连接，使用ROS2键盘控制FishBot移动时经常发生控制无响应问题，车载的OLED屏也频繁显示WIFI断连重新连接。
后学习至16.6节《做个遥控车-订阅ROS2 Twist》时，使用WIFI连接控制小车也出现控制无响应问题，遂怀疑是WIFI问题，并将程序改为通过串口发布控制指令，并添加OLED显示，显示当前车辆运行状况（线速度指令/角速度指令）
使用数据线连接小车，通过ROS2键盘控制时，按下i（前进键）小车开始前进，OLED显示屏显示当前线速度及角速度指令，但一段时间后小车自动停止，OLED屏幕显示清空，需重新按下键盘按键控制其继续移动（且有时无反应）导致控制不连续，查看运行microros-agent的终端发现会话，客户端，服务端，话题不停销毁重建

串口控制microros-agent界面显示如下

[1733226894.314018] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1733226894.314568] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1733226894.763491] info     | Root.cpp           | create_client            | create                 | client_key: 0x227F3C7A, session_id: 0x81
[1733226894.763565] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x227F3C7A, address: 0
[1733226894.786831] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x227F3C7A, participant_id: 0x000(1)
[1733226894.802584] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x227F3C7A, topic_id: 0x000(2), participant_id: 0x000(1)
[1733226894.813539] info     | ProxyClient.cpp    | create_subscriber        | subscriber created     | client_key: 0x227F3C7A, subscriber_id: 0x000(4), participant_id: 0x000(1)
[1733226894.826249] info     | ProxyClient.cpp    | create_datareader        | datareader created     | client_key: 0x227F3C7A, datareader_id: 0x000(6), subscriber_id: 0x000(4)
---------------------------------------------------------此处断连并自动销毁重建会话---------------------------------------------------------------------------------
[1733226933.706977] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x227F3C7A
[1733226933.707028] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x227F3C7A, address: 0
[1733226933.707059] info     | Root.cpp           | create_client            | create                 | client_key: 0x78542B7B, session_id: 0x81
[1733226933.707075] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x78542B7B, address: 0
[1733226933.720749] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x78542B7B, participant_id: 0x000(1)
[1733226933.736451] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x78542B7B, topic_id: 0x000(2), participant_id: 0x000(1)
[1733226933.747355] info     | ProxyClient.cpp    | create_subscriber        | subscriber created     | client_key: 0x78542B7B, subscriber_id: 0x000(4), participant_id: 0x000(1)
[1733226933.759909] info     | ProxyClient.cpp    | create_datareader        | datareader created     | client_key: 0x78542B7B, datareader_id: 0x000(6), subscriber_id: 0x000(4)

WIFI控制microros-agent界面显示如下

[1733227467.146393] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x08D2B914, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 05 00 05 00 80
---------------------------------------------------------此处发送多次消息但无响应---------------------------------------------------------------------------------
[1733227467.346800] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x08D2B914, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 05 00 05 00 80
[1733227467.547222] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x08D2B914, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 05 00 05 00 80
[1733227467.557690] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x08D2B914, len: 24, data: 
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 16 80 BD 26 81 00 FC 03
---------------------------------------------------------此处断连并自动销毁重建会话---------------------------------------------------------------------------------
[1733227467.586688] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x08D2B914
[1733227467.586739] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x08D2B914, address: 192.168.31.229:47138
[1733227467.586775] info     | Root.cpp           | create_client            | create                 | client_key: 0x1680BD26, session_id: 0x81
[1733227467.586792] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x1680BD26, address: 192.168.31.229:47138
[1733227467.586983] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x1680BD26, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1733227467.796858] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x1680BD26, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 00 00 00 3F 80
[1733227467.797311] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x1680BD26, len: 40, data: 
0000: 81 80 00 00 01 07 20 00 00 0A 00 01 01 03 00 00 12 00 00 00 00 01 00 00 0A 00 00 00 65 73 70 33
0020: 32 5F 63 61 72 00 00 00

补充

系统为ubuntu22.04,humble 双系统，非虚拟机
一直采用usb供电，未使用电池供电，不知是否有影响
WIFI使用小米路由器，未连接网线，单独提供给fishbot连接

#include <Arduino.h>
#include <Esp32McpwmMotor.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <geometry_msgs/msg/twist.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include "Wire.h"

// 定义 ROS2 执行器和支持结构
rclc_executor_t executor;
rclc_support_t support;
// 定义 ROS2 内存分配器
rcl_allocator_t allocator;
// 定义 ROS2 节点和订阅者
rcl_node_t node;
rcl_subscription_t subscriber;
// 定义接收到的消息结构体
geometry_msgs__msg__Twist sub_msg;
Adafruit_SSD1306 display(128, 64, &Wire); // 初始化 OLED 显示屏

// 定义控制两个电机的对象
Esp32McpwmMotor motor;

// 回调函数，当接收到新的 Twist 消息时会被调用
void twist_callback(const void *msg_in)
{
  // 将接收到的消息指针转化为 geometry_msgs__msg__Twist 类型
  const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in;
  // 从 Twist 消息中获取线速度和角速度
  float linear_x = twist_msg->linear.x;
  float angular_z = twist_msg->angular.z;

  // 更新 OLED 显示屏
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("Linear X: ");
  display.println(linear_x);
  display.print("Angular Z: ");
  display.println(angular_z);
  display.display();

  // 如果速度为零，则停止两个电机
  if (linear_x == 0 && angular_z == 0)
  {
    motor.updateMotorSpeed(0, 0);
    motor.updateMotorSpeed(1, 0);
    motor.updateMotorSpeed(2, 0);
    motor.updateMotorSpeed(3, 0);
    return;
  }

  // 根据线速度和角速度控制两个电机的转速
  if (linear_x > 0)
  {
    motor.updateMotorSpeed(0, 80);
    motor.updateMotorSpeed(1, 80);
    motor.updateMotorSpeed(2, 80);
    motor.updateMotorSpeed(3, 80);
  }
if (linear_x < 0)
  {
    motor.updateMotorSpeed(0, -80);
    motor.updateMotorSpeed(1, -80);
    motor.updateMotorSpeed(2, -80);
    motor.updateMotorSpeed(3, -80);
  }

  if (angular_z > 0)
  {
    motor.updateMotorSpeed(0, -80);
    motor.updateMotorSpeed(1, 80);
    motor.updateMotorSpeed(2, -80);
    motor.updateMotorSpeed(3, 80);
  }

  if (angular_z < 0)
  {
    motor.updateMotorSpeed(0, 80);
    motor.updateMotorSpeed(1, -80);
    motor.updateMotorSpeed(2, 80);
    motor.updateMotorSpeed(3, -80);
  }
}

void setup()
{
  Serial.begin(115200);
  // 设置通过串口进行MicroROS通信
  set_microros_serial_transports(Serial);
  // 延时时一段时间，等待设置完成
  delay(2000);
  // 初始化两个电机的引脚
  motor.attachMotor(0, 25, 33); // 左前
  motor.attachMotor(1, 26, 27); // 右前
  motor.attachMotor(2, 12, 22); // 左后
  motor.attachMotor(3, 16, 17); // 右后

  // 初始化 ROS2 执行器和支持结构
  allocator = rcl_get_default_allocator();
  Wire.begin(18, 19);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // 设置OLED的I2C地址，默认0x3C
  display.setTextSize(2);                    // 设置字体大小，最小为1
  display.setTextColor(SSD1306_WHITE);       // 设置字体颜色
  display.clearDisplay();                    // 清空屏幕
  display.display();                         // 显示内容

  rclc_support_init(&support, 0, NULL, &allocator);
  // 初始化 ROS2 节点
  rclc_node_init_default(&node, "esp32_car", "", &support);
  // 初始化订阅者
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      "/cmd_vel");
  rclc_executor_init(&executor, &support.context, 2, &allocator);

  // 设置订阅的回调函数
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &twist_callback, ON_NEW_DATA);
}

void loop()
{
  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)); // 循环处理数据
}

小鱼

@Mostima 在 microros-agent会话频繁断连，销毁重建中说：

一直采用usb供电，未使用电池供电，不知是否有影响

这个有影响，小米路由器我以前遇到过这个问题，如果是双频合一的，一定要拆开，电脑连网线或者5g，小车连2.4g

Mostima

@小鱼已解决，采用电池供电后控制正常，应该是串口供电同时启动电机，控制板供电不足重启导致的断连

小鱼

@Mostima ok

lyd199406

小鱼

@lyd199406 检查下网络延迟