9.4.1 第一个节点烧录完成后，机器人不停重启

liusiyu_alex_nuaa

9.4.1 第一个节点

main.cpp的内容如下：

#include <Arduino.h>
#include <Esp32McpwmMotor.h>
#include <Esp32PcntEncoder.h>
#include "PidController.h"
#include "Kinematics.h"

// 引入MicroROS 和 wifi的库

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

rcl_allocator_t allocator; // 内存分配器，动态内存分配管理
rclc_support_t support;    // 存储时钟，内存分配器和上下文，用于提供支持
rclc_executor_t executor;  // 执行器，用于管理订阅和计时器回调的执行
rcl_node_t node;           // 节点

// 单独创建一个任务运行micro-ROS 相当于一个线程
void microros_task(void* args)
{
    // 设置传输协议并延迟一段时间等待设置的完成
    IPAddress agent_ip;
    agent_ip.fromString("192.168.1.103");
    uint16_t agent_port = 8888;
    set_microros_wifi_transports("lsy","12345678",agent_ip,agent_port);
    delay(2000);
    // 初始化内存分配器
    allocator = rcl_get_default_allocator(); //获取默认的内存分配器
    // 初始化支持
    rclc_support_init(&support,0,NULL,&allocator);
    // 初始化节点
    rclc_node_init_default(&node, "robot_motion_control","",&support);
    // 初始化执行器
    unsigned int num_handles = 0;
    rclc_executor_init(&executor,&support.context,num_handles,&allocator);
    // 循环执行器
    rclc_executor_spin(&executor);

}

Esp32McpwmMotor motor;        // 创建一个名为motor的对象，用于控制电机
Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器
PidController pid_controller[2];
Kinematics kinematics;

float target_angular_speed = 0.1; // rad/s
float target_linear_speed = 50.0; // mm/s

float out_left_speed = 0.0;
float out_right_speed = 0.0;

void setup()
{
    Serial.begin(115200); // 初始化串口通信，波特率为115200

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

    // 初始化pid参数
    pid_controller[0].update_pid(0.625, 0.125, 0);
    pid_controller[1].update_pid(0.625, 0.125, 0);
    pid_controller[0].out_limit(-100, 100);
    pid_controller[1].out_limit(-100, 100);

    // 初始化运动学参数
    kinematics.set_wheel_distance(175);    // mm
    kinematics.set_motor_param(0, 0.1033); // 单个脉冲的行走距离 mm
    kinematics.set_motor_param(1, 0.1033);

    kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, out_left_speed, out_right_speed);
    Serial.printf("OUT:out_left_speed=%d,out_right_speed=%d\n", out_left_speed, out_right_speed);
    pid_controller[0].update_target(out_left_speed);
    pid_controller[1].update_target(out_right_speed);

    // 创建一个任务启动 micro_ROS
    // 第一个参数，运行的函数;第二个参数，任务的名字;第三个参数，分配内存的大小（字节）;第五个参数，优先级
    xTaskCreate(microros_task,"microros_task",10240,NULL,1,NULL);

}

void loop()
{
    delay(10); // 等待10毫秒
    // Serial.printf("current_speed1=%d,current_speed=%d\n", last_ticks[0], last_ticks[1]);
    kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks());
    motor.updateMotorSpeed(0, pid_controller[0].update(kinematics.get_motor_speed(0)));
    motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1)));
    // Serial.printf("OUT:out_left_speed=%d,out_right_speed=%d\n", kinematics.get_motor_speed(0),kinematics.get_motor_speed(1));
    Serial.printf("x=%f,y=%f,yaw=%f\n", kinematics.get_odom().x, kinematics.get_odom().y, kinematics.get_odom().angle);
}

编译过程出现wanning

src/main.cpp: In function 'void microros_task(void*)':
src/main.cpp:27:70: warning: ISO C++ forbids converting a string constant to 'char*' [-Wwrite-strings]
     set_microros_wifi_transports("lsy","12345678",agent_ip,agent_port);
                                                                      ^
src/main.cpp:27:70: warning: ISO C++ forbids converting a string constant to 'char*' [-Wwrite-strings]

串口输出

x=0.125146,y=0.017394,yaw=0.261623
x=0.125691,y=0.017540,yaw=0.262171
x=0.126183,y=0.017673,yaw=0.263314
Guru Meditation Error: Core  1 panic'ed (IllegalInstruction). Exception was unhandled.
Memory dump at 0x400d2cb0: 6cab6520 04adf01d e5201110
Core  1 register dump:
PC      : 0x400d2cb4  PS      : 0x00060730  A0      : 0x00000000  A1      : 0x3ffb4a70  
A2      : 0x3ffc3d40  A3      : 0x3ffc3cb8  A4      : 0x3ffc3d74  A5      : 0x00000000  
A6      : 0x00000000  A7      : 0x00000000  A8      : 0x800d2cb4  A9      : 0x3ffb4a50  
A10     : 0x00000001  A11     : 0xfffffffd  A12     : 0x00000000  A13     : 0x00000000  
A14     : 0x00000001  A15     : 0x3ffc3cb8  SAR     : 0x00000020  EXCCAUSE: 0x00000000  
EXCVADDR: 0x00000000  LBEG    : 0x40084681  LEND    : 0x40084689  LCOUNT  : 0x00000027  

Backtrace: 0x400d2cb1:0x3ffb4a70

ELF file SHA256: 49c2f071dd978ec8

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
OUT:out_left_speed=41.250000,out_right_speed=58.750000
x=0.000070,y=0.000000,yaw=0.000800
x=0.000119,y=0.000000,yaw=0.002480
x=0.000217,y=0.000001,yaw=0.003600
x=0.000217,y=0.000001,yaw=0.003600

机器人小伊

从你提供的信息来看，你的机器人项目在烧录第一个节点后出现了重启的问题，并且伴有“Guru Meditation Error: Core 1 panic'ed (IllegalInstruction)”的错误信息。这个错误通常表明ESP32在执行过程中遇到了非法的指令或访问了不允许的内存地址。以下是一些可能的原因和解决方案：

字符串常量转换为字符指针的警告：
你的代码中有一个警告，关于将字符串常量转换为char*。在C++中，字符串常量是const char*类型，而非常量char*。这可能不会导致直接的崩溃，但最好修正它以保持代码的健壮性。

修改方法：
```
set_microros_wifi_transports(const_cast<char*>("lsy"), const_cast<char*>("12345678"), agent_ip, agent_port);
```
但更好的做法是修改set_microros_wifi_transports函数的参数类型，使其接受const char*，如果可能的话。
内存访问问题：
Guru Meditation Error通常与内存访问错误相关，如空指针解引用、数组越界等。检查所有指针操作和数组访问，确保它们在有效范围内。
非法指令：
非法指令错误可能是由于编译选项不正确或代码中存在某些不被支持的指令。确保你的编译器设置和工具链是为ESP32优化的。
Micro-ROS相关：
- 确保Micro-ROS的配置正确，特别是与WiFi和Agent的连接设置。
- 检查Micro-ROS任务是否有可能导致系统资源耗尽（如内存泄漏）。
硬件问题：
- 检查ESP32的电源供应是否稳定。
- 确认所有外部设备（如电机和编码器）的连接正确无误，并且没有短路或损坏。
调试和日志：
- 增加更多的日志输出，特别是在loop()函数和Micro-ROS任务中，以帮助确定崩溃发生前的最后操作。
- 使用ESP32的调试工具（如JTAG调试器）进行更深入的调试。
软件更新：
- 确保你的开发环境（如PlatformIO, Arduino IDE等）和所有相关库都是最新的。
逐步排除：
- 尝试注释掉Micro-ROS相关的代码，看看问题是否仍然存在。这可以帮助确定问题是否与Micro-ROS相关。
- 逐一禁用其他功能（如PID控制、编码器读取等），以确定是否有特定的功能或代码块导致了问题。

通过上述步骤，你应该能够更接近问题的根源，并找到解决方案。如果问题仍然存在，可能需要更详细的代码审查和硬件检查。

以上为生成回答，仅供参考～

小鱼

@liusiyu_alex_nuaa 在 9.4.1 第一个节点烧录完成后，机器人不停重启中说：

unsigned int num_handles = 0;

将这个改成 1,或者忽略，下一节就好了

13920465364

@小鱼我也有相同的问题，并且修改成1也没用呀，板子还是会不断重启，是为什么呢？

13920465364

@liusiyu_alex_nuaa 楼主解决了吗？

sxjj

@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);
}