驱动板子一直重启

liusiyu_alex_nuaa

将以下程序烧录至驱动板后，一直重启，取消laser_joint_puber后，恢复正常

#include <Arduino.h>
#include <Esp32McpwmMotor.h>
#include <Esp32PcntEncoder.h>
#include "Robot_Vel_Control.h"
// 引入MicroROS 和 wifi的库

#include <WiFi.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 <nav_msgs/msg/odometry.h>
#include <std_msgs/msg/float32.h>
#include <sensor_msgs/msg/joint_state.h>
#include <micro_ros_utilities/string_utilities.h>

rcl_allocator_t allocator;             // 内存分配器，动态内存分配管理
rclc_support_t support;                // 存储时钟，内存分配器和上下文，用于提供支持
rclc_executor_t executor;              // 执行器，用于管理订阅和计时器回调的执行
rcl_node_t node;                       // 节点
rcl_subscription_t vel_suber;          // 速度订阅者
geometry_msgs__msg__Twist vel_sub_msg; // 速度订阅消息
rcl_publisher_t odom_puber;            // 里程计发布者
nav_msgs__msg__Odometry odom_pub_msg;  // 里程计消息
rcl_timer_t timer;                     // 定时器，定时调用某个函数
rcl_publisher_t laser_joint_puber;     // 下位机雷达角度反馈发布者

Robot_Vel_Control robot_vel_control;

float angular_speed = 0.0;
float linear_speed = 0.0;
float angular = 0.0;
float laser_angular = 0.234;

void odom_puber_callback(rcl_timer_t *timer, int64_t last_call_time)
{
    odom_t odom = robot_vel_control.get_odom();
    int64_t stamp = rmw_uros_epoch_millis(); // 获取当前时间

    odom_pub_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000);
    odom_pub_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp / 1000) * 10e6);
    odom_pub_msg.pose.pose.position.x = odom.x;
    odom_pub_msg.pose.pose.position.y = odom.y;
    odom_pub_msg.pose.pose.orientation.w = cos(odom.angle * 0.5);
    odom_pub_msg.pose.pose.orientation.x = 0;
    odom_pub_msg.pose.pose.orientation.y = 0;
    odom_pub_msg.pose.pose.orientation.z = sin(odom.angle * 0.5);
    odom_pub_msg.twist.twist.linear.x = odom.linear_speed;
    odom_pub_msg.twist.twist.angular.z = odom.angular_speed;

    if (rcl_publish(&odom_puber, &odom_pub_msg, NULL) != RCL_RET_OK)
    {
        Serial.printf("Error: odom publisher failed!\n");
    }
}

// sensor_msgs__msg__JointState类型初始化


// 发布 laser_joint
void laser_joint_puber_callback(rcl_timer_t *timer, int64_t last_call_time)
{
    sensor_msgs__msg__JointState laser_joint;

    int64_t stamp = rmw_uros_epoch_millis(); // 获取当前时间

    laser_joint.header.stamp.sec = static_cast<int32_t>(stamp / 1000);
    laser_joint.header.stamp.nanosec = static_cast<uint32_t>((stamp / 1000) * 10e6);
    laser_joint.header.frame_id = micro_ros_string_utilities_set(laser_joint.header.frame_id, "");
    laser_joint.name.capacity = 2;
    laser_joint.name.size = 2;
    laser_joint.name.data = (rosidl_runtime_c__String *)malloc(laser_joint.name.capacity * sizeof(rosidl_runtime_c__String));
    // laser_joint.name.data[0] = micro_ros_string_utilities_init_with_size(10);
    laser_joint.name.data[0] = micro_ros_string_utilities_set(laser_joint.name.data[0], "wheel_axis_joint");
    // laser_joint.name.data[1] = micro_ros_string_utilities_init_with_size(10);
    laser_joint.name.data[1] = micro_ros_string_utilities_set(laser_joint.name.data[1], "laser_base_joint");

    // 初始化关节速度
    laser_joint.velocity.size = 2;
    laser_joint.velocity.capacity = 2;
    laser_joint.velocity.data = (double *)malloc(laser_joint.velocity.size * sizeof(double));
    laser_joint.velocity.data[0] = 0.0;
    laser_joint.velocity.data[1] = 0.0;

    // 初始化关节力
    laser_joint.effort.size = 2;
    laser_joint.effort.capacity = 2;
    laser_joint.effort.data = (double *)malloc(laser_joint.effort.size * sizeof(double));
    laser_joint.effort.data[0] = 0.0;
    laser_joint.effort.data[1] = 0.0;

    // 初始化关节位置
    laser_joint.position.size = 2;
    laser_joint.position.capacity = 2;
    laser_joint.position.data = (double *)malloc(laser_joint.position.size * sizeof(double));

    laser_joint.position.data[0] = 0.11;
    laser_joint.position.data[1] = 0.22;

    if (rcl_publish(&laser_joint_puber, &laser_joint, NULL) != RCL_RET_OK)
    {
        Serial.printf("Error: laser_joint publisher failed!\n");
    }
}

void timer_50_callback(rcl_timer_t *timer, int64_t last_call_time)
{
    odom_puber_callback(timer, last_call_time);
    laser_joint_puber_callback(timer, last_call_time);
}

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 = 5;
    rclc_executor_init(&executor, &support.context, num_handles, &allocator);

    // 初始化订阅者，将订阅者加入到执行器当中
    rclc_subscription_init_best_effort(&vel_suber, &node,
                                       ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "cmd_vel");
    rclc_executor_add_subscription(&executor, &vel_suber, &vel_sub_msg, &twist_callback, ON_NEW_DATA);

    // 初始化发布者
    odom_pub_msg.child_frame_id = micro_ros_string_utilities_set(odom_pub_msg.child_frame_id, "base_footprint");
    odom_pub_msg.header.frame_id = micro_ros_string_utilities_set(odom_pub_msg.header.frame_id, "odom");
    rclc_publisher_init_best_effort(&odom_puber, &node,
                                    ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), "/odom");
    rclc_publisher_init_best_effort(&laser_joint_puber, &node,
                                    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState), "/joint_states");
    // 时间同步
    while (!rmw_uros_epoch_synchronized()) // 如果时间不同步
    {
        rmw_uros_sync_session(1000); // 同步时间
        delay(10);
    }
    // 将发布者加入到定时器
    rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(50), timer_50_callback);
    // 将定时器加入到执行器中
    rclc_executor_add_timer(&executor, &timer);
    // 循环执行器
    rclc_executor_spin(&executor);
}

void setup()
{
    // 初始化串口0通信，波特率为115200，8个数据位，偶校验，停止位2 串口监视器
    Serial.begin(115200, SERIAL_8E2);
    // 初始化串口1通信，波特率为115200，8个数据位，无校验，停止位1,rx22,tx23
    Serial2.setRxBufferSize(1024);
    Serial2.begin(115200, SERIAL_8N1, 22, 23);
    // 注册串口1的中断服务函数
    Serial2.onReceive(Serial2_Receive_Callback);
    Serial.onReceive(Serial_Receive_Callback);
    // 创建一个任务启动 micro_ROS
    // 第一个参数，运行的函数;第二个参数，任务的名字;第三个参数，分配内存的大小（字节）;第五个参数，优先级
    xTaskCreate(microros_task, "microros_task", 20480, NULL, 1, NULL);
}

void loop()
{
    delay(10); // 等待10毫秒
    robot_vel_control.update_odom(millis(), linear_speed, angular_speed, angular);
    // 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", robot_vel_control.get_odom().x, robot_vel_control.get_odom().y, robot_vel_control.get_odom().angle);
}

机器人小伊

根据你提供的代码和问题描述，当laser_joint_puber（雷达角度反馈发布者）启用时，驱动板一直重启，而取消该发布者后系统恢复正常。这种情况通常指示存在内存管理、资源分配或消息处理上的问题。以下是一些可能的原因和解决方案：

内存泄漏或分配不足：
- 在laser_joint_puber_callback中，你为laser_joint消息的name和velocity、effort、position数组动态分配了内存。如果这些内存没有被正确释放，可能会导致内存泄漏。虽然Arduino/ESP32通常具有垃圾回收机制（在MicroPython等环境中更常见），但在C++中手动管理内存是很重要的。
- 解决方案：在不再需要这些消息时（例如，在microros_task函数结束或某个清理函数中），释放这些动态分配的内存。使用free()函数来释放之前通过malloc()分配的内存。
发布消息过大：
- 如果laser_joint消息因为包含大量数据而变得过大，它可能会超出系统能够处理或传输的极限。
- 解决方案：检查laser_joint消息的大小，确保它不会过大。如果必要，减少消息中的数据量或增加系统的处理能力（例如，增加内存或优化通信协议）。
Micro-ROS配置问题：
- 可能是Micro-ROS的配置或初始化过程中存在问题，导致在尝试使用laser_joint_puber时系统不稳定。
- 解决方案：检查Micro-ROS的初始化代码，确保所有组件都正确配置和初始化。此外，检查是否有任何与WiFi通信或代理节点相关的配置错误。
硬件问题：
- 硬件故障（如电源不稳定、内存芯片故障等）也可能导致系统重启。
- 解决方案：检查硬件连接和电源供应。如果可能，尝试在不同的硬件上运行相同的代码以排除硬件故障。
调试和日志：
- 增加更多的调试输出和日志记录，特别是在与laser_joint_puber相关的代码部分。这可以帮助识别问题发生的具体位置。
- 解决方案：在关键代码路径中添加Serial.printf()语句来打印变量值和函数返回结果，从而跟踪程序的执行流程。
软件依赖和库：
- 确保所有使用的库都与你的开发环境和硬件兼容，并且是最新版本。
- 解决方案：检查并更新所有相关的库和依赖项。

通过上述步骤，你应该能够定位并解决问题。如果问题依旧存在，可能需要更深入地分析代码或寻求来自Micro-ROS社区或ESP32开发者的帮助。

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