烧录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♪(･ω･)ﾉ！！！