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

小鱼

@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`节点