四驱板使用fishbot四驱代码，无法正常发送odom和imu话题

Plaech

@小鱼编码器的问题我解决了，是我的线接错了[笑哭]，但是imu还是不行，我直接读取imu数据发送至串口可以正常发送的，说明imu没有问题，应该是代码问题。之前看到imu和屏幕似乎使用了同一个引脚，我认为有冲突于是把显示器的代码注释了，可依然不行，然后我发现以下代码中imu_msg.angular_velocity.x出现重复，也进行了修改，但是imu话题依然没有正确发布。之后，我把if (imu.isEnable())进行了注释，依然不发布imu话题。

if (imu.isEnable())
        {
            imu.getImuDriverData(imu_data);

            imu_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分
            imu_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分

            imu_msg.angular_velocity.x = imu_data.angular_velocity.x;
            imu_msg.angular_velocity.x = imu_data.angular_velocity.x;
            imu_msg.angular_velocity.x = imu_data.angular_velocity.x;

            imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x;
            imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y;
            imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z;

            imu_msg.orientation.x = imu_data.orientation.x;
            imu_msg.orientation.y = imu_data.orientation.y;
            imu_msg.orientation.z = imu_data.orientation.z;
            imu_msg.orientation.w = imu_data.orientation.w;

            RCSOFTCHECK(rcl_publish(&imu_publisher, &imu_msg, NULL));
        }

之后，我把if (imu.isEnable())进行了注释，依然不发布imu话题。我认为其他文件没有出错，因为imu相关的文件应该只有ImuDriver.h和ImuDriver.cpp还有MPU6050_light.h，而这些文件是直接copy github的，应该不会出错，所以问题大概率为fishbot.cpp的问题，以下是我的全部代码(说实话，对照着二驱板的开源来来回回看了很多次了，不知道问题在哪，编译也不报错。)

/**
 * @file fishbot.cpp
 * @author fishros (fishros@foxmail.com)
 * @brief 核心文件，硬件控制以及通讯控制
 * @version V1.0.0
 * @date 2023-01-04
 *
 * @copyright Copyright (c) fishros.com & fishros.org.cn 2023
 *
 */
#include "fishbot.h"

/*==================MicroROS消息============================*/
sensor_msgs__msg__Imu imu_msg;
geometry_msgs__msg__Twist twist_msg;
nav_msgs__msg__Odometry odom_msg;
char odom_frame_id[16];
fishbot_interfaces__srv__FishBotConfig_Response config_res;
fishbot_interfaces__srv__FishBotConfig_Request config_req;
static micro_ros_utilities_memory_conf_t conf = {0};
static int keyvalue_capacity = 100;

/*==================MicroROS订阅发布者服务========================*/
rcl_publisher_t odom_publisher;
rcl_publisher_t imu_publisher;       // 用于发布机器人的IMU信息（Imu）
rcl_subscription_t twist_subscriber;
rcl_service_t config_service;
rcl_wait_set_t wait_set;

/*==================MicroROS相关执行器&节点===================*/
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;
enum states
{
    WAITING_AGENT,
    AGENT_AVAILABLE,
    AGENT_CONNECTED,
    AGENT_DISCONNECTED
} state;

/*========================FishBot控制相关====================*/
PidController pid_controller[4];
Esp32PcntEncoder encoders[4];
Esp32McpwmMotor motor;
Kinematics kinematics;
FishBotConfig config;
//FishBotDisplay display;
BluetoothSerial SerialBT;
float battery_voltage;
OneButton button(0, true);
MPU6050 mpu(Wire);               // 初始化MPU6050对象
ImuDriver imu(mpu);              // 初始化Imu对象
imu_t imu_data;                  // IMU 数据对象

void WiFiEventCB(WiFiEvent_t event)
{
    Serial.println("WIFI EVENT!");
    switch (event)
    {
    case SYSTEM_EVENT_STA_GOT_IP:
        //display.updateWIFIIp(WiFi.localIP().toString());
        break;
    case SYSTEM_EVENT_STA_LOST_IP:
        //display.updateWIFIIp("wait connect!");
        break;
    };
}

void doubleClick()
{
    fishlog_debug("key", "doubleClick() detected.");
    if (config.microros_transport_mode() == CONFIG_TRANSPORT_MODE_WIFI_UDP_CLIENT)
    {
        config.config("microros_mode", "serial");
    }
    else
    {
        config.config("microros_mode", "udp_client");
    }
    esp_restart();
}

bool setup_fishbot()
{
    // 1.初始化
    Serial.begin(115200);
    fishlog_set_target(Serial);
    config.init(CONFIG_NAME_NAMESPACE);
    Serial.println(FIRST_START_TIP);
    Serial.println(config.config_str());
    //display.init();
    //display.updateTransMode(config.microros_transport_mode());
    //display.updateStartupInfo();
    button.attachDoubleClick(doubleClick);
    // 2.设置IO 电机&编码器
    motor.attachMotor(0, 12, 22);
    motor.attachMotor(1, 16, 17);
    motor.attachMotor(2, 25, 33);
    motor.attachMotor(3, 26, 27);

    encoders[0].init(0, 14, 23);
    encoders[1].init(1, 15, 13);
    encoders[2].init(2, 36, 39);
    encoders[3].init(3, 32, 35);
    imu.begin(18, 19);//imu初始化

    // 3.设置PID
    for (int i = 0; i < 4; i++)
    {
        pid_controller[i].update_target(0.0);
        pid_controller[i].update_pid(config.kinematics_pid_kp(), config.kinematics_pid_ki(), config.kinematics_pid_kd());
        pid_controller[i].out_limit(-config.kinematics_pid_out_limit(), config.kinematics_pid_out_limit());
        kinematics.set_motor_param(i, config.kinematics_reducation_ration(), config.kinematics_pulse_ration(), config.kinematics_wheel_diameter());
    }
    // 4.设置运动学参数
    kinematics.set_kinematic_param(config.kinematics_wheel_distance());
    // 7.设置电压测量引脚
    pinMode(34, INPUT);
    analogReadResolution(12);
    analogSetAttenuation(ADC_11db);
    battery_voltage = 5.02 * ((float)analogReadMilliVolts(34) * 1e-3);
    return true;
}

static void deal_command(char key[32], char value[32])
{
    if (strcmp(key, "command") == 0)
    {
        if (strcmp(value, "restart") == 0)
        {
            esp_restart();
        }
        else if (strcmp(value, "read_config") == 0)
        {
            Serial.print(config.config_str());
        }
        return;
    }
    else
    {
        String recv_key(key);
        String recv_value(value);
        config.config(recv_key, recv_value);
        Serial.print("$result=ok\n");
    }
}

bool setup_fishbot_transport()
{
    bool setup_success = true;
    if (config.microros_transport_mode() == CONFIG_TRANSPORT_MODE_WIFI_UDP_CLIENT)
    {
        fishlog_set_target(Serial);
        WiFi.onEvent(WiFiEventCB);
        setup_success = microros_setup_transport_udp_client_();
        //display.updateTransMode("udp_client");
    }
    if (config.microros_transport_mode() == CONFIG_TRANSPORT_MODE_SERIAL)
    {
        SerialBT.begin(config.board_name());
        fishlog_set_target(SerialBT);
        microros_setup_transport_serial_();
        //display.updateTransMode("serial");
    }

    return true;
}

bool create_fishbot_transport()
{
    String nodename = config.ros2_nodename();
    String ros2namespace = config.ros2_namespace();
    String twist_topic = config.ros2_twist_topic_name();
    String odom_topic = config.ros2_odom_topic_name();
    String odom_frameid_str = config.ros2_odom_frameid();
    String odom_child_frameid_str = config.ros2_odom_child_frameid();
    odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, odom_frameid_str.c_str());
    odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, odom_child_frameid_str.c_str());
    imu_msg.header.frame_id = micro_ros_string_utilities_set(imu_msg.header.frame_id, "imu");
    odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, odom_frameid_str.c_str());
    const unsigned int timer_timeout = config.odom_publish_period();
    delay(500);
    allocator = rcl_get_default_allocator();
    // 调用 rclc_timer_init_default 函数初始化 ROS 2 定时器，传入支持库、定时器周期和回调函数
    RCSOFTCHECK(rclc_publisher_init_best_effort(
        &imu_publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
       "imu"));
    RCSOFTCHECK(rclc_support_init(&support, 0, NULL, &allocator));
    RCSOFTCHECK(rclc_node_init_default(&node, nodename.c_str(), ros2namespace.c_str(), &support));
    RCSOFTCHECK(rclc_publisher_init_best_effort(
        &odom_publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
        odom_topic.c_str()));
    RCSOFTCHECK(rclc_subscription_init_default(
        &twist_subscriber,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
        twist_topic.c_str()));
    RCSOFTCHECK(rclc_service_init_default(
        &config_service,
        &node,
        ROSIDL_GET_SRV_TYPE_SUPPORT(fishbot_interfaces, srv, FishBotConfig),
        "/fishbot_config"));
    RCSOFTCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(timer_timeout), callback_sensor_publisher_timer_));
    //RCSOFTCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(timer_timeout), callback_odom_publisher_timer_));
    RCSOFTCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator));
    RCSOFTCHECK(rclc_executor_add_subscription(&executor, &twist_subscriber, &twist_msg, &callback_twist_subscription_, ON_NEW_DATA));
    RCSOFTCHECK(rclc_executor_add_timer(&executor, &timer));
    RCSOFTCHECK(rclc_executor_add_service(&executor, &config_service, &config_req, &config_res, callback_config_service_));
    
    return true;
}

bool destory_fishbot_transport()
{
    rmw_context_t *rmw_context = rcl_context_get_rmw_context(&support.context);
    (void)rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);

    RCSOFTCHECK(rcl_publisher_fini(&odom_publisher, &node));
    RCSOFTCHECK(rcl_publisher_fini(&imu_publisher, &node));
    RCSOFTCHECK(rcl_subscription_fini(&twist_subscriber, &node));
    RCSOFTCHECK(rcl_service_fini(&config_service, &node));
    RCSOFTCHECK(rcl_timer_fini(&timer));
    RCSOFTCHECK(rclc_executor_fini(&executor));
    RCSOFTCHECK(rcl_node_fini(&node));
    rclc_support_fini(&support);
    return true;
}

void loop_fishbot_control()
{
    static float out_motor_speed[4];
    static uint64_t last_update_info_time = millis();
    static uint8_t index = 0;
    kinematics.update_motor_ticks(micros(), encoders[0].getTicks(), encoders[1].getTicks(), encoders[2].getTicks(), encoders[3].getTicks());
    for (index = 0; index < 4; index++)
    {
        // 目标速度为0时停止控制，解决 #https://fishros.org.cn/forum/topic/1372 问题
        if (pid_controller[index].target_ == 0)
        {
            out_motor_speed[index] = 0;
        }
        else
        {
            // 使用 pid_controller 控制器对电机速度进行 PID 控制
            out_motor_speed[index] = pid_controller[index].update(kinematics.motor_speed(index));
        }
        // 将 PID 控制器的输出值作为电机的目标速度进行控制
        motor.updateMotorSpeed(index, out_motor_speed[index]);
        // fishlog_debug("pid", "index:%d target:%f current:%f out=%f", index, pid_controller[index].target_, kinematics.motor_speed(index), out_motor_speed[index]);
    }

    // 电量信息
    if (out_motor_speed[0] == 0 && out_motor_speed[1] == 0)
    {
        battery_voltage = 5.02 * ((float)analogReadMilliVolts(34) * 1e-3);
        //display.updateBatteryInfo(battery_voltage);
    }
    // 更新系统信息
    //display.updateCurrentTime(rmw_uros_epoch_millis());
    //display.updateDisplay();
    button.tick();
    imu.update();
}

void loop_fishbot_transport()
{
    static char result[10][32];
    static int config_result;
    while (Serial.available())
    {
        int c = Serial.read();
        config_result = config.loop_config_uart(c, result);
        if (config_result == CONFIG_PARSE_OK)
        {
            deal_command(result[0], result[1]);
        }
        else if (config_result == CONFIG_PARSE_ERROR)
        {
            Serial.print("$result=error parse\n");
        }
    }

    switch (state)
    {
    case WAITING_AGENT:
        EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;);
        break;
    case AGENT_AVAILABLE:
        state = (true == create_fishbot_transport()) ? AGENT_CONNECTED : WAITING_AGENT;
        if (state == WAITING_AGENT)
        {
            destory_fishbot_transport();
        };
        break;
    case AGENT_CONNECTED:
        EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;);
        if (state == AGENT_CONNECTED)
        {
            if (!rmw_uros_epoch_synchronized())
            {
                RCSOFTCHECK(rmw_uros_sync_session(1000));
                if (rmw_uros_epoch_synchronized())
                {
                    setTime(rmw_uros_epoch_millis() / 1000 + SECS_PER_HOUR * 8);
                    fishlog_debug("fishbot", "current_time:%ld", rmw_uros_epoch_millis());
                }
                delay(10);
                return;
            }
            RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
        }
        break;
    case AGENT_DISCONNECTED:
        destory_fishbot_transport();
        state = WAITING_AGENT;
        break;
    default:
        break;
    }

    if (state != AGENT_CONNECTED)
    {
        delay(10);
    }
}

bool microros_setup_transport_udp_client_()
{
    String ssid = config.wifi_sta_ssid();
    String ip = config.microros_uclient_server_ip();
    String password = config.wifi_sta_pswd();
    uint32_t agent_port = config.microros_uclient_server_port();

    // 初始化数据接收配置
    config_req.key = micro_ros_string_utilities_init_with_size(keyvalue_capacity);
    config_req.value = micro_ros_string_utilities_init_with_size(keyvalue_capacity);
    config_res.key = micro_ros_string_utilities_init_with_size(keyvalue_capacity);
    config_res.value = micro_ros_string_utilities_init_with_size(keyvalue_capacity);

    IPAddress agent_ip;
    agent_ip.fromString(ip);
    if (!set_microros_wifi_transports((char *)ssid.c_str(), (char *)password.c_str(), agent_ip, agent_port, config.board_name()))
    {
        return false;
    }
    return true;
}

bool microros_setup_transport_serial_()
{
    String nodename = config.ros2_nodename();
    String ros2namespace = config.ros2_namespace();
    String twist_topic = config.ros2_twist_topic_name();
    String odom_topic = config.ros2_odom_topic_name();
    String odom_frameid_str = config.ros2_odom_frameid();
    // 用于将odom_frameid_str的C字符串值设置为odom_msg的帧ID。
    // 该函数会将odom_msg中的帧ID指针所指向的字符串缓冲区中的数据替换为odom_frameid_str的C字符串值，并返回指向新字符串的指针。
    // 这样可以确保odom_msg的帧ID与配置中的值一致。
    odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, odom_frameid_str.c_str());
    odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, odom_frameid_str.c_str());
    imu_msg.header.frame_id = micro_ros_string_utilities_set(imu_msg.header.frame_id, "imu");
    const unsigned int timer_timeout = config.odom_publish_period();
    // 初始化数据接收配置
    config_req.key = micro_ros_string_utilities_init_with_size(keyvalue_capacity);
    config_req.value = micro_ros_string_utilities_init_with_size(keyvalue_capacity);
    config_res.key = micro_ros_string_utilities_init_with_size(keyvalue_capacity);
    config_res.value = micro_ros_string_utilities_init_with_size(keyvalue_capacity);
    uint32_t serial_baudraate = config.serial_baudrate();
    Serial.updateBaudRate(serial_baudraate);
    if (!set_microros_serial_transports(Serial))
    {
        return false;
    }
    return true;
}

// void callback_odom_publisher_timer_(rcl_timer_t *timer, int64_t last_call_time)
// {
//     RCLC_UNUSED(last_call_time);
//     if (timer != NULL)
//     {
//         odom_t odom = kinematics.odom();
//         int64_t stamp = rmw_uros_epoch_millis();
//         odom_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分
//         odom_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分
//         odom_msg.pose.pose.position.x = odom.x;
//         odom_msg.pose.pose.position.y = odom.y;
//         odom_msg.pose.pose.orientation.w = odom.quaternion.w;
//         odom_msg.pose.pose.orientation.x = odom.quaternion.x;
//         odom_msg.pose.pose.orientation.y = odom.quaternion.y;
//         odom_msg.pose.pose.orientation.z = odom.quaternion.z;
//         odom_msg.twist.twist.angular.z = odom.angular_speed;
//         odom_msg.twist.twist.linear.x = odom.linear_x_speed;
//         odom_msg.twist.twist.linear.y = odom.linear_y_speed;

//         display.updateBotAngular(odom.angular_speed);
//         display.updateBotLinear(odom.linear_x_speed);

//         RCSOFTCHECK(rcl_publish(&odom_publisher, &odom_msg, NULL));

//         if (imu.isEnable())
//         {
//             imu.getImuDriverData(imu_data);

//             imu_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分
//             imu_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分

//             imu_msg.angular_velocity.x = imu_data.angular_velocity.x;
//             imu_msg.angular_velocity.x = imu_data.angular_velocity.x;
//             imu_msg.angular_velocity.x = imu_data.angular_velocity.x;

//             imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x;
//             imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y;
//             imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z;

//             imu_msg.orientation.x = imu_data.orientation.x;
//             imu_msg.orientation.y = imu_data.orientation.y;
//             imu_msg.orientation.z = imu_data.orientation.z;
//             imu_msg.orientation.w = imu_data.orientation.w;

//             RCSOFTCHECK(rcl_publish(&imu_publisher, &imu_msg, NULL));
//         }
//     }
// }

void callback_twist_subscription_(const void *msgin)
{
    const geometry_msgs__msg__Twist *msg = (const geometry_msgs__msg__Twist *)msgin;
    static float target_motor_speed1, target_motor_speed2, target_motor_speed3, target_motor_speed4;
    kinematics.kinematic_inverse(msg->linear.x * 1000, msg->linear.y * 1000, msg->angular.z,
                                 target_motor_speed1, target_motor_speed2, target_motor_speed3, target_motor_speed4);

    pid_controller[0].update_target(target_motor_speed1);
    pid_controller[1].update_target(target_motor_speed2);
    pid_controller[2].update_target(target_motor_speed3);
    pid_controller[3].update_target(target_motor_speed4);
}

void callback_config_service_(const void *req, void *res)
{
    fishbot_interfaces__srv__FishBotConfig_Request *req_in = (fishbot_interfaces__srv__FishBotConfig_Request *)req;
    fishbot_interfaces__srv__FishBotConfig_Response *res_in = (fishbot_interfaces__srv__FishBotConfig_Response *)res;
    String recv_key(req_in->key.data);
    String recv_value(req_in->value.data);
    fishlog_debug("fishbot", "recv service key=%s,value=%s\n", req_in->key.data, req_in->value.data);
    config.config(recv_key, recv_value);
    // PID相关配置
    if (recv_key.startsWith("pid_"))
    {
        pid_controller[0].update_pid(config.kinematics_pid_kp(), config.kinematics_pid_ki(), config.kinematics_pid_kd());
        pid_controller[1].update_pid(config.kinematics_pid_kp(), config.kinematics_pid_ki(), config.kinematics_pid_kd());
        pid_controller[0].out_limit(-config.kinematics_pid_out_limit(), config.kinematics_pid_out_limit());
        pid_controller[1].out_limit(-config.kinematics_pid_out_limit(), config.kinematics_pid_out_limit());
    }
    // 系统相关配置
    if (recv_key.equalsIgnoreCase("system"))
    {
        if (recv_value.equalsIgnoreCase("restart"))
        {
            xTaskCreate([](void *param)
                        {for(int i=0;i<3;i++)delay(1000);esp_restart(); },
                        "restart_task", 1024, NULL, 1, NULL);
        }
    }
    fishlog_debug("fishbot", "current_str:%s", config.config_str().c_str());
    sprintf(res_in->key.data, "%s", req_in->key.data);
    sprintf(res_in->value.data, "%s", req_in->key.data);
}

void callback_sensor_publisher_timer_(rcl_timer_t *timer, int64_t last_call_time)
{
    RCLC_UNUSED(last_call_time);
    if (timer != NULL)
    {
        // 用于获取当前的时间戳，并将其存储在消息的头部中
        int64_t stamp = rmw_uros_epoch_millis();
        // 获取机器人的位置和速度信息，并将其存储在一个ROS消息（odom_msg）中
        odom_t odom = kinematics.odom();
        odom_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分
        odom_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分
        odom_msg.pose.pose.position.x = odom.x;
        odom_msg.pose.pose.position.y = odom.y;
        odom_msg.pose.pose.orientation.w = odom.quaternion.w;
        odom_msg.pose.pose.orientation.x = odom.quaternion.x;
        odom_msg.pose.pose.orientation.y = odom.quaternion.y;
        odom_msg.pose.pose.orientation.z = odom.quaternion.z;

        odom_msg.twist.twist.angular.z = odom.angular_speed;
        odom_msg.twist.twist.linear.x = odom.linear_x_speed;
        odom_msg.twist.twist.linear.y = odom.linear_y_speed;

        //display.updateBotAngular(odom.angular_speed);
        //display.updateBotLinear(odom.linear_x_speed);
        //display.updateBotLinear(odom.linear_y_speed);

        RCSOFTCHECK(rcl_publish(&odom_publisher, &odom_msg, NULL));

        //if (imu.isEnable())
        //{
            imu.getImuDriverData(imu_data);

            imu_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分
            imu_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分

            imu_msg.angular_velocity.x = imu_data.angular_velocity.x;
            imu_msg.angular_velocity.y = imu_data.angular_velocity.y;
            imu_msg.angular_velocity.z = imu_data.angular_velocity.z;

            imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x;
            imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y;
            imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z;

            imu_msg.orientation.x = imu_data.orientation.x;
            imu_msg.orientation.y = imu_data.orientation.y;
            imu_msg.orientation.z = imu_data.orientation.z;
            imu_msg.orientation.w = imu_data.orientation.w;

            RCSOFTCHECK(rcl_publish(&imu_publisher, &imu_msg, NULL));
        //}
    }
}
```@小鱼

小鱼

@2429148505 我也没有发现问题，暂时看不出来，后续我抽时间将开源的代码在四驱版上测试修改，添加IMU后再来回帖。

Plaech

@小鱼在四驱板使用fishbot四驱代码，无法正常发送odom和imu话题中说：

@2429148505 我也没有发现问题，暂时看不出来，后续我抽时间将开源的代码在四驱版上测试修改，添加IMU后再来回帖。

催催，我自己也试了很久，还是解决不了。尝试过在fishros那个框架下，只发布imu话题，一直成功不了。您这边有什么进展了吗。有没有发现以上的代码的问题。

Plaech

@小鱼以下是我只测试imu能否正常发布的fishbot.cpp文件，main函数与imu,microROS库都与您在github上的开源相同，仍然无法正常发布imu话题。您能帮我看看问题，或者教教我debug的步骤吗。这个问题困扰我很久了

#include "fishbot.h"
#include "TimeLib.h"
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
#define SECS_PER_HOUR ((time_t)(3600UL))
sensor_msgs__msg__Imu imu_msg;
rcl_publisher_t imu_publisher;       // 用于发布机器人的IMU信息（Imu）
fishbot_interfaces__srv__FishBotConfig_Response config_res;
fishbot_interfaces__srv__FishBotConfig_Request config_req;
MPU6050 mpu(Wire);               // 初始化MPU6050对象
ImuDriver imu(mpu);              // 初始化Imu对象
imu_t imu_data;                  // IMU 数据对象
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;
const unsigned int timer_timeout = 50;
enum states
{
    WAITING_AGENT,
    AGENT_AVAILABLE,
    AGENT_CONNECTED,
    AGENT_DISCONNECTED
} state;
int8_t split_str(const char *line, char result[][32])
{
    if (line[0] != '$')
        return 1;
    uint16_t index = 0;
    uint16_t count = 0;
    uint16_t temp_index = 0;
    for (index = 1; line[index] != '\0'; index++)
    {
        if (line[index] == '=')
        {
            result[count++][temp_index++] = '\0';
            temp_index = 0;
            continue;
        }
        result[count][temp_index++] = line[index];
    }
    result[count][temp_index++] = '\0';

    if (count != 1)
    {
        return 1;
    }
    return 1;
}
int8_t loop_config_uart(int c, char result[][32])
{
    static char line[512];
    static int index = 0;
    if (c == '\n')
    {
        line[index] = '\0';
        index = 0;
        return split_str(line, result);
    }
    else if (c > 0 && c < 127)
    {
        line[index] = c;
        ++index;
    }
    return 0;
}
bool setup_fishbot()
{
    Serial.begin(115200);
    //fishlog_set_target(Serial);
    //config.init("fishbot");
    Serial.println("FIRST_START_TIP");
    //Serial.println(config.config_str());
    imu.begin(18, 19);//imu初始化
    return 1;
}
static void deal_command(char key[32], char value[32])
{
    if (strcmp(key, "command") == 0)
    {
        if (strcmp(value, "restart") == 0)
        {
            esp_restart();
        }
        else if (strcmp(value, "read_config") == 0)
        {
            Serial.print("Ok");
        }
        return;
    }
    else
    {
        String recv_key(key);
        String recv_value(value);
        //config.config(recv_key, recv_value);
        Serial.print("$result=ok\n");
    }
}
bool setup_fishbot_transport()
{
    bool setup_success = true;
    
    
    //SerialBT.begin("config.board_name()");
    //fishlog_set_target(SerialBT);
    microros_setup_transport_serial_();
    //display.updateTransMode("serial");
    

    return true;
}
bool create_fishbot_transport()
{
//     String odom_child_frameid_str = //config.ros2_odom_child_frameid();
//     odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, odom_frameid_str.c_str());
//     odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, odom_child_frameid_str.c_str());
    imu_msg.header.frame_id = micro_ros_string_utilities_set(imu_msg.header.frame_id, "imu");
    RCSOFTCHECK(rclc_publisher_init_best_effort(
        &imu_publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
       "imu"));
    RCSOFTCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(timer_timeout), callback_sensor_publisher_timer_));
    RCSOFTCHECK(rclc_executor_add_timer(&executor, &timer));
    return true;
}
bool destory_fishbot_transport()
{
    rmw_context_t *rmw_context = rcl_context_get_rmw_context(&support.context);
    (void)rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);
    RCSOFTCHECK(rcl_publisher_fini(&imu_publisher, &node));
    return 1;
}
void loop_fishbot_control()
{
    imu.update();
}
void loop_fishbot_transport()
{
    static char result[10][32];
    static int config_result;
    while (Serial.available())
    {
        int c = Serial.read();
        config_result = loop_config_uart(c, result);
        if (config_result == 1)
        {
            deal_command(result[0], result[1]);
        }
        else if (config_result == -1)
        {
            Serial.print("$result=error parse\n");
        }
    }

    switch (state)
    {
    case WAITING_AGENT:
        EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;);
        break;
    case AGENT_AVAILABLE:
        state = (true == create_fishbot_transport()) ? AGENT_CONNECTED : WAITING_AGENT;
        if (state == WAITING_AGENT)
        {
            destory_fishbot_transport();
        };
        break;
    case AGENT_CONNECTED:
        EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;);
        if (state == AGENT_CONNECTED)
        {
            if (!rmw_uros_epoch_synchronized())
            {
                RCSOFTCHECK(rmw_uros_sync_session(1000));
                if (rmw_uros_epoch_synchronized())
                {
                    setTime(rmw_uros_epoch_millis() / 1000 + SECS_PER_HOUR * 8);
                    //fishlog_debug("fishbot", "current_time:%ld", rmw_uros_epoch_millis());
                }
                delay(10);
                return;
            }
            RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
        }
        break;
    case AGENT_DISCONNECTED:
        destory_fishbot_transport();
        state = WAITING_AGENT;
        break;
    default:
        break;
    }

    if (state != AGENT_CONNECTED)
    {
        delay(10);
    }
}
bool microros_setup_transport_serial_(){
    const unsigned int timer_timeout = 50;
    imu_msg.header.frame_id = micro_ros_string_utilities_set(imu_msg.header.frame_id, "imu");
    if (!set_microros_serial_transports(Serial))
    {
        return false;
    }
    return true;
}
void callback_sensor_publisher_timer_(rcl_timer_t *timer, int64_t last_call_time)
{
    int64_t stamp = rmw_uros_epoch_millis();
    imu.getImuDriverData(imu_data);

    imu_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分
    imu_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分

    imu_msg.angular_velocity.x = imu_data.angular_velocity.x;
    imu_msg.angular_velocity.y = imu_data.angular_velocity.y;
    imu_msg.angular_velocity.z = imu_data.angular_velocity.z;

    imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x;
    imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y;
    imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z;

    imu_msg.orientation.x = imu_data.orientation.x;
    imu_msg.orientation.y = imu_data.orientation.y;
    imu_msg.orientation.z = imu_data.orientation.z;
    imu_msg.orientation.w = imu_data.orientation.w;

    RCSOFTCHECK(rcl_publish(&imu_publisher, &imu_msg, NULL));
}

Plaech

@小鱼我整理好上传github了，速速审核

小鱼

@2429148505 收到

小鱼

@2429148505 已合并，但四驱版本不久前分割出新的仓库了： https://github.com/fishros/fishbot_motion_control_microros_four_driver

如果有空可以顺便同步一下，万分感谢！

Plaech

@小鱼 OK,有空我上传一下。顺便问问，我想搞麦轮利用imu的姿态矫正，有什么推荐的资料吗

小鱼

@2429148505 在ROS 2中，融合IMU和里程计数据可以使用多种方式和工具包。常用的方法是通过机器人定位和导航工具包（如robot_localization），它能够处理多个传感器输入并提供融合后的定位信息。

以下是一个基本的步骤指南，帮助你在ROS 2中实现IMU和里程计数据的融合：

1. 安装ROS 2和依赖包

确保你已经安装了ROS 2和相关的依赖包。可以参考ROS 2的官方安装指南来安装ROS 2。

2. 安装`robot_localization`包

robot_localization是一个非常强大的工具包，用于融合多个传感器数据，包括IMU和里程计。

sudo apt-get update
sudo apt-get install ros-<your-ros2-distro>-robot-localization

请将<your-ros2-distro>替换为你所使用的ROS 2发行版名称，如foxy、galactic或humble。

3. 配置IMU和里程计数据发布

确保你的IMU和里程计数据分别发布到正确的ROS主题上。通常情况下：

IMU数据发布到/imu/data主题
里程计数据发布到/odom主题

你可以使用ROS 2的发布者和订阅者来发布这些数据。

4. 创建配置文件

创建一个YAML配置文件，用于配置robot_localization节点的参数。以下是一个示例配置文件ekf.yaml：

ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.1
    two_d_mode: true
    publish_tf: true
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    odom0: /odom
    odom0_config: [true, true, false,
                   false, false, true,
                   false, false, false,
                   false, false, false,
                   false, false, false]
    odom0_queue_size: 10

    imu0: /imu/data
    imu0_config: [false, false, false,
                  true, true, false,
                  false, false, false,
                  false, false, true,
                  false, false, false]
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true

5. 启动`robot_localization`节点

创建一个启动文件，使用launch来启动robot_localization节点，并加载你的配置文件。以下是一个示例启动文件ekf_launch.py：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node',
            output='screen',
            parameters=['/path/to/your/ekf.yaml'],
        )
    ])

将/path/to/your/ekf.yaml替换为你的ekf.yaml文件的实际路径。

6. 运行你的ROS 2应用

使用ros2 launch命令启动你的应用：

ros2 launch <your_package_name> ekf_launch.py

将<your_package_name>替换为包含启动文件的实际包名。

7. 验证融合结果

你可以使用rviz2或其他可视化工具来查看融合后的定位结果。确保定位结果是预期的，并根据需要调整配置文件中的参数。

通过这些步骤，你应该能够在ROS 2中成功融合IMU和里程计数据，以实现更精确的机器人定位和导航。

Plaech

@小鱼我的意思是，我现在的麦轮在控制层面会出现右偏的情况，不能正常地直走。想在控制算法层面上改进。不过在定位算法上，我没有实践过。定位的更准并持续地更新路径，能改善这个问题吗。

Plaech

@小鱼不过在定位方面我最近在考虑LIO-SAM算法，到时候看看效果怎么样。

小鱼

@2429148505 在四驱板使用fishbot四驱代码，无法正常发送odom和imu话题中说：

在的麦轮在控制层面会出现右偏的情况

校准轮距和轮子的编码器数值到距离的转换因子就好

Plaech

@小鱼在四驱板使用fishbot四驱代码，无法正常发送odom和imu话题中说：

@2429148505 在四驱板使用fishbot四驱代码，无法正常发送odom和imu话题中说：

在的麦轮在控制层面会出现右偏的情况

校准轮距和轮子的编码器数值到距离的转换因子就好

没有听懂，如果是编码器到距离没有校准的问题，那直走应该不会右偏吧。我现在直走也不正常。还是说我理解的不对，有相关的资料吗

小鱼

@2429148505 和二驱差不多，就是速度计算那里，每个轮子因为出场原因，减速比并不一样造成的误差

Plaech

@小鱼 ok,我试试，确实没搞校准

Plaech

@小鱼在四驱板使用fishbot四驱代码，无法正常发送odom和imu话题中说：

@2429148505 和二驱差不多，就是速度计算那里，每个轮子因为出场原因，减速比并不一样造成的误差

帮你加上设置矫正脉冲的功能了，有空的时候上传

小鱼

@2429148505 ok

Plaech

@小鱼最近测试四驱板，发现我的odom方向与实际方向相反。(我说之前导航怎么老是出问题，我还以为是打滑的原因(虽然还是走不直，以后打算加一个imu纠偏))使用的是fishbot四驱板的代码，以下是现象:当cmd_vel只给x速度时，小车会向前走，而且四个轮子输出都是正的，可以排除是安装错误的问题，但是轮子向前转动时，输出的脉冲为负值，而且返回的速度也都为负值。而且不是编码器读数应该没问题，因为把编码器的引脚改反，会发生错误，轮子会因为pid原地振动。感觉是因为正解算时解算反了，但是我找不到代码错误。不知道你有没有出现这个现象。

Plaech

@2429148505 在四驱板使用fishbot四驱代码，无法正常发送odom和imu话题中说：

@小鱼最近测试四驱板，发现我的odom方向与实际方向相反。(我说之前导航怎么老是出问题，我还以为是打滑的原因(虽然还是走不直，以后打算加一个imu纠偏))使用的是fishbot四驱板的代码，以下是现象:当cmd_vel只给x速度时，小车会向前走，而且四个轮子输出都是正的，可以排除是安装错误的问题，但是轮子向前转动时，输出的脉冲为负值，而且返回的速度也都为负值。而且不是编码器读数应该没问题，因为把编码器的引脚改反，会发生错误，轮子会因为pid原地振动。感觉是因为正解算时解算反了，但是我找不到代码错误。不知道你有没有出现这个现象。

好像不是这个原因。我记得单独给pid控制器目标值的时候，给正值也是向前转(与脉冲方向相反)，应该不是解算问题，也不是安装问题，是pid控制器的问题。而且还有一个现象，就是给yaw角速度时，旋转的方向也是相反的。

Plaech

@2429148505 在四驱板使用fishbot四驱代码，无法正常发送odom和imu话题中说：

@2429148505 在四驱板使用fishbot四驱代码，无法正常发送odom和imu话题中说：

@小鱼最近测试四驱板，发现我的odom方向与实际方向相反。(我说之前导航怎么老是出问题，我还以为是打滑的原因(虽然还是走不直，以后打算加一个imu纠偏))使用的是fishbot四驱板的代码，以下是现象:当cmd_vel只给x速度时，小车会向前走，而且四个轮子输出都是正的，可以排除是安装错误的问题，但是轮子向前转动时，输出的脉冲为负值，而且返回的速度也都为负值。而且不是编码器读数应该没问题，因为把编码器的引脚改反，会发生错误，轮子会因为pid原地振动。感觉是因为正解算时解算反了，但是我找不到代码错误。不知道你有没有出现这个现象。

好像不是这个原因。我记得单独给pid控制器目标值的时候，给正值也是向前转(与脉冲方向相反)，应该不是解算问题，也不是安装问题，是pid控制器的问题。而且还有一个现象，就是给yaw角速度时，旋转的方向也是相反的。

好像就是我的编码器ab弄反了，导致一直加速。至于抖动现象，不知道是什么原因。

四驱板使用fishbot四驱代码，无法正常发送odom和imu话题

1. 安装ROS 2和依赖包

2. 安装robot_localization包

3. 配置IMU和里程计数据发布

4. 创建配置文件

5. 启动robot_localization节点

6. 运行你的ROS 2应用

7. 验证融合结果

2. 安装`robot_localization`包

5. 启动`robot_localization`节点