烧录9.4.1或者9.4.2程序后,机器人反复重启，无法创建micro-ros任务

[13920465364]

/* Get all possible data from MPU6050
 * Accelerometer values are given as multiple of the gravity [1g = 9.81 m/s²]
 * Gyro values are given in deg/s
 * Angles are given in degrees
 * Note that X and Y are tilt angles and not pitch/roll.
 *
 * License: MIT
 */

#include <Arduino.h>
#include <micro_ros_platformio.h>
#include <WiFi.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include "Wire.h"
#include <MPU6050_light.h>
#include <Esp32McpwmMotor.h>
#include <Esp32PcntEncoder.h>
#include "geometry_msgs/msg/twist.h"  //消息接口

#include "PidController.hpp"
#include "Kinematics.hpp"


//定义一些与micro相关的结构体对象
rcl_allocator_t allocator;  //用于内存分配
rclc_support_t support;     //用于存储时钟，为内存分配器和上下文提供支持
rclc_executor_t executor;   //用于管理订阅和计时器回调的执行
rcl_node_t node;            //节点
rcl_subscription_t sub_cmd_vel; //订阅者
geometry_msgs__msg__Twist msg_cmd_vel; //消息对象


Esp32McpwmMotor motor;
Esp32PcntEncoder encoder[2];    // 两个编码器
MyPidController pidcontroller[2];
Kinematics kinematics;

float out_line_speed = 50; // mm/s
float out_angle_speed = 0.1;  // '/s  弧度每秒
float left_speed = 0;
float right_speed = 0;

// int64_t pre_time = 0;
// int64_t last_ticks[2] = {0, 0};
// int64_t delta_tick[2] = {0, 0};
// float current_speed[2] = {0, 0};


void twist_callback(const void *msgin)
{
  const geometry_msgs__msg__Twist *msg = (const geometry_msgs__msg__Twist *)msgin;
  // 线速度
  out_line_speed = msg->linear.x * 1000; // m/s 转化成 mm/s
  // 角速度
  out_angle_speed = msg->angular.z; // 弧度每秒

  //计算目标线速度角速度下的左右轮速度
  kinematics.kinematics_inverse(out_line_speed, out_angle_speed, &left_speed, &right_speed);
  Serial.printf("Target speed: {left motor speed: %f, right motor speed: %f}\n", left_speed, right_speed);

  pidcontroller[0].update_target(left_speed); //设置目标速度, 这里为0.15m/s
  pidcontroller[1].update_target(right_speed); //设置目标速度

}


//pid测试函数
// void motoSpeedControl() {
//   // //获取当前轮上速度
//   int64_t dt = millis() - pre_time; //ms
//   pre_time = millis();
//   delta_tick[0] = encoder[0].getTicks() - last_ticks[0];
//   delta_tick[1] = encoder[1].getTicks() - last_ticks[1];
//   current_speed[0] = delta_tick[0] * 0.103233 / dt * 1000; //mm/s
//   current_speed[1] = delta_tick[1] * 0.103233 / dt * 1000;
//   last_ticks[0] = encoder[0].getTicks();
//   last_ticks[1] = encoder[1].getTicks();
//   pre_time = millis();
//   //调用pid控制器计算输入电机的占空比, update函数返回一个小数，但是这里要求输入整形，因此这里会对float进行截断
//   motor.updateMotorSpeed(0, (int16_t) pidcontroller[0].update(current_speed[0]));
//   motor.updateMotorSpeed(1, (int16_t) pidcontroller[1].update(current_speed[1]));
// }

// 线程处理函数，用于处理micro-ros的任务
void micro_ros_task(void *arg) {
  // 1.设置传输协议，建立连接
  IPAddress agent_ip; //即上位机的ip
  agent_ip.fromString("192.168.0.191");
  // 连接wifi
  set_microros_wifi_transports("super", "88888888", agent_ip, 8888);
  delay(3000);
  // 2.初始化内存分配器
  allocator = rcl_get_default_allocator();
  // 3.初始化rclc支持结构体
  rclc_support_init(&support, 0, NULL, &allocator);
  // 4.创建节点
  rclc_node_init_default(&node, "fishbot_motor_control", "", &support);
  // 5.初始化执行器
  unsigned int num_handles = 1; // 订阅者和定时器的数量
  rclc_executor_init(&executor, &support.context, num_handles, &allocator);
  // 初始化订阅者，并将其添加到执行器中
  rclc_subscription_init_best_effort(&sub_cmd_vel, &node, 
    ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), 
    "/cmd_vel");
  rclc_executor_add_subscription(&executor, &sub_cmd_vel, &msg_cmd_vel, &twist_callback, ON_NEW_DATA);
  // 6.进入循环，处理micro-ros任务
  rclc_executor_spin(&executor);
}


void setup()
{
  Serial.begin(115200);

  //初始化电机和编码器
  motor.attachMotor(0, 22, 23); // 电机1连接到引脚22和23
  motor.attachMotor(1, 12, 13); // 电机2连接到引脚12和13
  encoder[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接
  encoder[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接

  //pid控制器初始化
  pidcontroller[0].update_pid(0.625, 0.125, 0); //设置PID参数
  pidcontroller[1].update_pid(0.625, 0.125, 0); //设置PID参数
  pidcontroller[0].out_limit(-100, 100); //设置输出限制
  pidcontroller[1].out_limit(-100, 100); //设置输出限制

  //设置运动学参数初始化
  kinematics.set_motor_param(0, 0.103233); //左轮每个脉冲对应的距离
  kinematics.set_motor_param(1, 0.103233); //右轮每个脉冲对应的距离
  kinematics.set_wheel_distance(175.0); //轮间距175mm
  //计算目标线速度角速度下的左右轮速度
  kinematics.kinematics_inverse(out_line_speed, out_angle_speed, &left_speed, &right_speed);
  Serial.printf("Target speed: {left motor speed: %f, right motor speed: %f}\n", left_speed, right_speed);

  pidcontroller[0].update_target(left_speed);
  pidcontroller[1].update_target(right_speed);


  // 创建一个任务来处理micro-ros的通信
  // 以下两种方式使用过后均导致小车重启
  xTaskCreate(micro_ros_task, "microros_task", 10240, NULL, 1, NULL);
  // xTaskCreatePinnedToCore(micro_ros_task, "microros_task", 10240, NULL, 1, NULL, 1);
}




void loop()
{

  delay(10);

  //pid测试
  // motoSpeedControl();
  // Serial.printf("Current speed: {left motor speed: %f, right motor speed: %f}\n", 
  //   current_speed[0], current_speed[1]);

  kinematics.update_motor_speed(millis(), encoder[0].getTicks(), encoder[1].getTicks());
  //调用pid控制器计算输入pwm的占空比, update函数返回一个小数，但是这里要求输入整形，因此这里会对float进行截断
  motor.updateMotorSpeed(0, (int16_t) pidcontroller[0].update(kinematics.get_motor_speed(0)));
  motor.updateMotorSpeed(1, (int16_t) pidcontroller[1].update(kinematics.get_motor_speed(1)));

  // 打印两轮速度
  // Serial.printf("Current speed: {left motor speed: %f, right motor speed: %f}\n", 
  //   kinematics.get_motot_speed(0), kinematics.get_motot_speed(1));
  // 打印里程计信息
  Serial.printf("Odom: {x: %f, y: %f, angle: %f}\n", 
    kinematics.get_odom().x, kinematics.get_odom().y, kinematics.get_odom().angle);

}

已经测试过，使用这条语句xTaskCreate(micro_ros_task, "microros_task", 10240, NULL, 1, NULL);添加任务后，板子运行一小段时间就会自动重启，注释掉这条语句板子的能正常运行并且功能正常。

重启时串口打印信息如下：

Odom: {x: 0.172388, y: 0.032440, angle: 0.361021}
Odom: {x: 0.172919, y: 0.032642, angle: 0.362791}
Odom: {x: 0.173449, y: 0.032844, angle: 0.364560}
Odom: {x: 0.173931, y: 0.033029, angle: 0.365740}
Guru Meditation Error: Core  0 panic'ed (IllegalInstruction). Exception was unhandled.

Core  0 register dump:
PC      : 0x6d6f682f  PS      : 0x00060530  A0      : 0x801451f9  A1      : 0x3ffb4830  
A2      : 0x3ffcd378  A3      : 0x3ffb4930  A4      : 0x00000018  A5      : 0x3ffb486f  
A6      : 0x3ffcd411  A7      : 0x00000054  A8      : 0x8015943c  A9      : 0x3ffb47f0  
A10     : 0x3ffc3f78  A11     : 0x3ffb4930  A12     : 0x00000018  A13     : 0x00000001  
A14     : 0x00000000  A15     : 0x00000001  SAR     : 0x0000001b  EXCCAUSE: 0x00000000  
EXCVADDR: 0x00000000  LBEG    : 0x400899d0  LEND    : 0x400899e6  LCOUNT  : 0xffffffff  


Backtrace: 0x6d6f682c:0x3ffb4830 |<-CORRUPTED




ELF file SHA256: 69f1953c41a5adb4

Rebooting...
ets Jul 29 2019 12:21:46

rst:0xc (SW_CPU_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT)
configsip: 0, SPIWP:0xee
clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00
mode:DIO, clock div:2
load:0x3fff0030,len:1184
load:0x40078000,len:13232
load:0x40080400,len:3028
entry 0x400805e4
Target speed: {left motor speed: 41.250000, right motor speed: 58.750000}
Odom: {x: 0.000619, y: 0.000000, angle: 0.000000}
Odom: {x: 0.001239, y: 0.000000, angle: 0.000000}
Odom: {x: 0.001910, y: 0.000001, angle: 0.001770}
Odom: {x: 0.002323, y: 0.000002, angle: 0.001770}
Odom: {x: 0.002736, y: 0.000003, angle: 0.002950}
Odom: {x: 0.003097, y: 0.000004, angle: 0.003539}
Odom: {x: 0.003355, y: 0.000005, angle: 0.004129}
Odom: {x: 0.003665, y: 0.000007, angle: 0.005309}

该问题与这篇问题帖子所描述情况相同，并且无法通过修改num_handles为1来解决。

大佬帮忙看看，Thanks♪(･ω･)ﾉ！！！

[sxjj]

@13920465364 @liusiyu_alex_nuaa 我吐了啊，我测试了一下我是因为防火墙问题，我是wsl用户。。。。
在任务函数中每段加个检查看看，再在setup函数中加个延迟5s，确保串口能接收到消息。查查看wifi连上没，质量怎么样。此外测试的时候别忘了小车按按复位。

void microros_task(void *args)
{
    // 1.设置传输协议并延迟一段时间等待设置的完成。
    IPAddress agent_ip; // 用来存储 IP 地址。
    agent_ip.fromString("xxxxxxx"); // 电脑/手机当前的IP地址。
    set_microros_wifi_transports("xxx", "xxxxxxxx", agent_ip, 8888); // 设置传输协议。告诉机器人连名称为“xxx”的WiFi，密码为“xxxxxxx”。之后去往代理服务器的IP地址为xxxxxxxxx，端口号为8888的地方发消息。
    Serial.print("WiFi status: ");
    Serial.println(WiFi.status() == WL_CONNECTED ? "Connected" : "Failed"); // 判断wifi是否连接上，ture则"Connected"，false则"Failed"。
    if (WiFi.status() == WL_CONNECTED) {
        Serial.print("IP: ");
        Serial.println(WiFi.localIP());
    }
    delay(2000); // 等待2秒，等待wifi连接。
    
    // 2.初始化内存分配器。
    allocator = rcl_get_default_allocator(); // 获取默认的内存分配器。
    
    // 3.初始化支持，并检查错误。
    if (rclc_support_init(&support, 0, NULL, &allocator) != RCL_RET_OK) { Serial.println("support init failed"); }
    // 初始化支持。使用这个默认的内存分配器 allocator（它使用标准的 malloc 和 free），在没有任何命令行参数的情况下，帮我初始化好 ROS 2 客户端库的底层环境，并把所有初始化好的状态信息都保存在 support 这个结构体里，供我后续创建节点使用。
    
    // 4.初始化节点，并检查错误。
    if (rclc_node_init_default(&node, "fishbot_motion_control", "", &support) != RCL_RET_OK) { Serial.println("node init failed"); }
    // 初始化节点。在由 support 提供的运行环境中，创建一个名为 fishbot_motion_control 的节点，它位于全局命名空间（""），并把这个节点的所有状态信息保存在 node 这个结构体里。
    
    // 5.初始化执行器，并检查错误。
    unsigned int num_handles = 1; // 订阅和计时器的数量，注意这是一个要改的参数（句柄的数量）。大概，比如我设置5，它就有了5个槽位，编号0-4。现在比如0-3我填满了（有东西要执行），他就会去依次询问0-3有没有执行（有没有消息），没执行要去执行他们对应的功能（有消息回调）。句柄 = 唯一识别号。
    if (rclc_executor_init(&executor, &support.context, 1, &allocator) != RCL_RET_OK) { Serial.println("executor init failed"); }
    // 初始化执行器。使用 allocator 这个内存分配器，在 support.context 提供的 ROS 通信上下文里，初始化一个执行器 executor，让它能够管理 num_handles 个句柄（比如x个订阅者或定时器）。
    
    // 6.初始化订阅者，并将其添加到执行器中。
    rclc_subscription_init_best_effort(&sub_cmd_vel, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "/cmd_vel"); // 订阅者句柄地址，节点句柄地址，消息类型支持（什么类型的消息）（地址），话题名称（订阅哪个话题）。ROSIDL_GET_MSG_TYPE_SUPPORT 是 ROS 2 C语言客户端库（rclc）中一个非常重要的预处理宏，它就像是订阅者与特定消息类型之间的"桥梁"或"身份证"。
    rclc_executor_add_subscription(&executor, &sub_cmd_vel, &msg_cmd_vel, twist_callback, ON_NEW_DATA); // 执行器句柄地址，订阅者句柄地址，存放接收到的消息的内存空间的地址（消息缓冲区），回调函数，触发条件：有新数据时触发，一收到消息就执行回调。
    // 7.循环执行器。
    rclc_executor_spin(&executor); // 循环执行器。它会持续地、反复地检查所有已添加到执行器的句柄（如订阅者、定时器），并处理它们对应的回调函数。只要 ROS 2 的上下文（context）保持有效，这个循环就不会停止.

    // // spin 返回说明上下文已失效;不要跑完任务,直接挂起:
    // while (1) { vTaskDelay(pdMS_TO_TICKS(1000)); } // 或 vTaskDelete(NULL);
}