亚博科技和幻尔科技的十轴IMU在Ros2 Humble下驱动后数值无限趋于0的解决方案

news2024/11/19 1:49:39

在做机器人导航以及建模的时候，考虑到多传感器融合可能会带来更好的效果，于是决定使用幻尔科技的十轴IMU（其实亚博科技与幻尔科技这块IMU的内部完全一致，驱动代码都完全一致）驱动后使用以下命令输出传来的四元数等数据。

$ ros2 launch wit_ros2_imu rviz_and_imu.launch.py

起初我以为是我数据传输问题，经过检查波特率等发现没有问题，经过使劲晃动IMU数据依然很小，带有e-05次方等，无限趋向于0。查阅网上的文章发现也有人遇到了这个问题，但是并非Humble版本，他给予了那个版本的解决方案，我迁移到Humble版本，并且直接在驱动得到的数据做了一个卡尔曼滤波（其实是因为如果要对IMU的ROS2数据加滤波又要使用另外一个包IMU_Tools）。

这里直接贴代码了，Ros2 Humble版本的驱动，要注意端口Port以及波特率与上位机一致，不然得不到数据。

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(gnss_imu_sim)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(sensor_msgs REQUIRED)

# 手动设置serial库路径

set(SERIAL_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/../serial/include)
set(SERIAL_LIBRARY ${CMAKE_SOURCE_DIR}/../serial/build/libserial.a)

add_executable(imu_driver_exe src/imu.cpp)
target_include_directories(imu_driver_exe PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
  ${SERIAL_INCLUDE_DIR}) # 添加serial库头文件路径
target_link_libraries(imu_driver_exe ${SERIAL_LIBRARY}) # 链接serial库文件
target_compile_features(imu_driver_exe PUBLIC c_std_99 cxx_std_17)

ament_target_dependencies(
  imu_driver_exe
  "rclcpp"
  "std_msgs"
  "sensor_msgs"
  "tf2"
)

install(TARGETS imu_driver_exe DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>gnss_imu_sim</name>
  <version>0.0.0</version>
  <description>A ROS 2 package for GNSS and IMU simulation.</description>
  <maintainer email="zard@todo.todo">zard</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>tf2</depend>
  <depend>sensor_msgs</depend>

  <depend>serial</depend> <!-- Assuming the 'serial' library is required -->

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

src/imu.cpp

#include <iostream>
#include <chrono>
#include <cmath>
#include <vector>
#include <thread>
#include <atomic>
#include <serial/serial.h>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/magnetic_field.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include "rclcpp/rclcpp.hpp"

#define BAUDRATE 9600

std::atomic_bool imu_thread_running;
std::atomic_bool imu_data_ready;
std::vector<uint8_t> buff;
std::vector<int16_t> acceleration(4, 0);
std::vector<int16_t> angularVelocity(4, 0);
std::vector<int16_t> magnetometer(4, 0);
std::vector<int16_t> angle_degree(4, 0);

// 卡尔曼滤波器类
class KalmanFilter
{
public:
    KalmanFilter(double process_noise, double measurement_noise, double estimation_error)
        : process_noise_(process_noise), measurement_noise_(measurement_noise), estimation_error_(estimation_error), last_estimation_(0)
    {
    }

    double update(double measurement)
    {
        double kalman_gain = estimation_error_ / (estimation_error_ + measurement_noise_);
        double current_estimation = last_estimation_ + kalman_gain * (measurement - last_estimation_);
        estimation_error_ = (1 - kalman_gain) * estimation_error_ + fabs(last_estimation_ - current_estimation) * process_noise_;
        last_estimation_ = current_estimation;
        return current_estimation;
    }

private:
    double process_noise_;
    double measurement_noise_;
    double estimation_error_;
    double last_estimation_;
};

// 每种传感器数据的卡尔曼滤波器实例
std::vector<KalmanFilter> acc_filters(4, KalmanFilter(1e-2, 1e-1, 1.0));
std::vector<KalmanFilter> gyro_filters(4, KalmanFilter(1e-2, 1e-1, 1.0));
std::vector<KalmanFilter> mag_filters(4, KalmanFilter(1e-2, 1e-1, 1.0));
std::vector<KalmanFilter> angle_filters(4, KalmanFilter(1e-2, 1e-1, 1.0));

class IMUDriverNode : public rclcpp::Node
{
public:
    IMUDriverNode(const std::string& nodeName) : Node(nodeName)
    {
        // 获取串口
        _port_name = this->declare_parameter<std::string>("port_name", "/dev/imu_usb");
        // 发布IMU数据
        publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu/data_raw", 10);
        // 发布磁力计数据
        mag_publisher_ = this->create_publisher<sensor_msgs::msg::MagneticField>("/imu/mag", 10);
        // IMU驱动线程
        imu_thread_ = std::thread(&IMUDriverNode::imuThread, this, _port_name);
    }

    void joinIMUThread()
    {
        imu_thread_.join();
    }

private:
    void imuThread(const std::string &port_name)
    {
        serial::Serial imu_serial;
        try
        {
            imu_serial.setPort(port_name);
            imu_serial.setBaudrate(BAUDRATE);
            serial::Timeout timeout = serial::Timeout::simpleTimeout(500);
            imu_serial.setTimeout(timeout);
            imu_serial.open();
            RCLCPP_INFO(this->get_logger(), "\033[32mSerial port opened successfully...\033[0m");
        }
        catch (const serial::IOException &e)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to open the IMU serial port.");
            return;
        }

        imu_serial.flush();

        while (rclcpp::ok() && imu_thread_running.load())
        {
            if (imu_serial.available())
            {
                uint8_t data;
                imu_serial.read(&data, 1);
                buff.push_back(data);

                if (buff.size() >= 11 && buff[0] == 0x55)
                {
                    std::vector<uint8_t> data_buff(buff.begin(), buff.begin() + 11);
                    std::vector<uint8_t> data(buff.begin() + 2, buff.begin() + 10);
                    bool angle_flag = false;

                    if (data_buff[1] == 0x51)
                    {
                        if (checkSum(data_buff))
                        {
                            acceleration = hexToShort(data);
                            RCLCPP_DEBUG(this->get_logger(), "Acceleration data updated.");
                        }
                        else
                        {
                            RCLCPP_WARN(this->get_logger(), "0x51 Check failure.");
                        }
                    }
                    else if (data_buff[1] == 0x52)
                    {
                        if (checkSum(data_buff))
                        {
                            angularVelocity = hexToShort(data);
                            RCLCPP_DEBUG(this->get_logger(), "Angular velocity data updated.");
                        }
                        else
                        {
                            RCLCPP_WARN(this->get_logger(), "0x52 Check failure.");
                        }
                    }
                    else if (data_buff[1] == 0x53)
                    {
                        if (checkSum(data_buff))
                        {
                            angle_degree = hexToShort(data);
                            angle_flag = true;
                            RCLCPP_DEBUG(this->get_logger(), "Angle data updated.");
                        }
                        else
                        {
                            RCLCPP_WARN(this->get_logger(), "0x53 Check failure.");
                        }
                    }
                    else if (data_buff[1] == 0x54)
                    {
                        if (checkSum(data_buff))
                        {
                            magnetometer = hexToShort(data);
                            RCLCPP_DEBUG(this->get_logger(), "Magnetometer data updated.");
                        }
                        else
                        {
                            RCLCPP_WARN(this->get_logger(), "0x54 Check failure.");
                        }
                    }

                    buff.clear();

                    if (angle_flag)
                    {
                        imu_data_ready.store(true);
                        RCLCPP_DEBUG(this->get_logger(), "IMU data ready for publishing.");
                    }
                }
                else if (buff[0] != 0x55)
                {
                    buff.clear();
                }
            }

            if (imu_data_ready.load())
            {
                // 更新滤波数据
                std::vector<double> acc_filtered(4, 0);
                std::vector<double> gyro_filtered(4, 0);
                std::vector<double> mag_filtered(4, 0);
                std::vector<double> angle_filtered(4, 0);

                for (size_t i = 0; i < 4; i++)
                {
                    acc_filtered[i] = acc_filters[i].update(acceleration[i]);
                    gyro_filtered[i] = gyro_filters[i].update(angularVelocity[i]);
                    mag_filtered[i] = mag_filters[i].update(magnetometer[i]);
                    angle_filtered[i] = angle_filters[i].update(angle_degree[i]);
                }

                // 发布IMU数据
                sensor_msgs::msg::Imu imu_msg;
                imu_msg.header.stamp = this->now();
                imu_msg.header.frame_id = "base_link";

                imu_msg.linear_acceleration.x = static_cast<double>(acc_filtered[0]) / 32768.0 * 16 * 9.8;
                imu_msg.linear_acceleration.y = static_cast<double>(acc_filtered[1]) / 32768.0 * 16 * 9.8;
                imu_msg.linear_acceleration.z = static_cast<double>(acc_filtered[2]) / 32768.0 * 16 * 9.8;

                imu_msg.angular_velocity.x = static_cast<double>(gyro_filtered[0]) / 32768.0 * 2000 * M_PI / 180;
                imu_msg.angular_velocity.y = static_cast<double>(gyro_filtered[1]) / 32768.0 * 2000 * M_PI / 180;
                imu_msg.angular_velocity.z = static_cast<double>(gyro_filtered[2]) / 32768.0 * 2000 * M_PI / 180;

                double roll = static_cast<double>(angle_filtered[0]) / 32768.0 * M_PI;
                double pitch = static_cast<double>(angle_filtered[1]) / 32768.0 * M_PI;
                double yaw = static_cast<double>(angle_filtered[2]) / 32768.0 * M_PI;
                tf2::Quaternion quat;
                quat.setRPY(roll, pitch, yaw);

                imu_msg.orientation.x = quat.getX();
                imu_msg.orientation.y = quat.getY();
                imu_msg.orientation.z = quat.getZ();
                imu_msg.orientation.w = quat.getW();

                publisher_->publish(imu_msg);
                RCLCPP_INFO(this->get_logger(), "IMU data published.");

                // 发布磁力计数据
                sensor_msgs::msg::MagneticField mag_msg;
                mag_msg.header.stamp = this->now();
                mag_msg.header.frame_id = "base_link";
                mag_msg.magnetic_field.x = static_cast<double>(mag_filtered[0]);
                mag_msg.magnetic_field.y = static_cast<double>(mag_filtered[1]);
                mag_msg.magnetic_field.z = static_cast<double>(mag_filtered[2]);

                mag_publisher_->publish(mag_msg);
                RCLCPP_INFO(this->get_logger(), "Magnetometer data published.");

                imu_data_ready.store(false);
            }
        }
        imu_serial.close();
    }

    bool checkSum(const std::vector<uint8_t> &data_buff)
    {
        uint8_t sum = 0;
        for (size_t i = 0; i < data_buff.size() - 1; i++)
        {
            sum += data_buff[i];
        }

        return sum == data_buff[data_buff.size() - 1];
    }

    std::vector<int16_t> hexToShort(const std::vector<uint8_t> &hex_data)
    {
        std::vector<int16_t> short_data(4, 0);
        for (size_t i = 0; i < hex_data.size(); i += 2)
        {
            int16_t high = static_cast<int16_t>(hex_data[i + 1]);
            short_data[i / 2] = static_cast<int16_t>((high << 8) | hex_data[i]);
        }

        return short_data;
    }

    std::string _port_name;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
    rclcpp::Publisher<sensor_msgs::msg::MagneticField>::SharedPtr mag_publisher_;
    std::thread imu_thread_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto imu_node = std::make_shared<IMUDriverNode>("imu_driver_node");
    imu_thread_running.store(true);

    rclcpp::spin(imu_node);
    imu_thread_running.store(false);

    imu_node->joinIMUThread();

    rclcpp::shutdown();
    return 0;
}

launch/imu_driver_launch.py

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='gnss_imu_sim',
            executable='imu_driver_exe',
            name='imu_driver_exe',
            parameters=[{"port_name": "/dev/imu_usb"}],  # Removed the tilde
            output='both'
        )
    ])

注意，要安装serial包Davesh98/系列： ROS2_HUMBLE (github.com)

最好把它放在imu_driver_exe，也就是src的上级目录，这样Cmake里的相对位置不会报错，省事了很多。修改了驱动文件可以看到数据的单位正常了，趋势也正常了。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/1955363.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

自写ApiTools工具，功能参考Postman和ApiPost

自写ApiTools工具，功能参考Postman和ApiPost

近日在使用ApiPost的时候&#xff0c;发现新版本8和7不兼容&#xff0c;也就是说8不支持离线操作&#xff0c;而7可以。 我想说&#xff0c;我就是因为不想登录使用才从Postman换到ApiPost的。 众所周知&#xff0c;postman时国外软件&#xff0c;登录经常性抽风&#xff0c;…
阅读更多...
Mike SHE里如何正确设置分区降雨

Mike SHE里如何正确设置分区降雨

前言&#xff1a; MIKE SHE分布式水文模型现阶段用于流域洪水的项目比较多&#xff0c;因属于大尺度模型&#xff0c;基本可以模拟水循环全过程&#xff0c;包含降雨—蒸发——产汇流—地表水—地下水等。同时还可以耦合MIKE11水动力水质模型。 今天给大家介绍下MIKESHE是如何…
阅读更多...
从零到一使用 Ollama、Dify 和 Docker 构建 Llama 3.1 模型服务

从零到一使用 Ollama、Dify 和 Docker 构建 Llama 3.1 模型服务

本篇文章聊聊&#xff0c;如何使用 Ollama、Dify 和 Docker 来完成本地 Llama 3.1 模型服务的搭建。 如果你需要将 Ollama 官方不支持的模型运行起来&#xff0c;或者将新版本 llama.cpp 转换的模型运行起来&#xff0c;并且想更轻松的使用 Dify 构建 AI 应用&#xff0c;那么…
阅读更多...
进程间的通信（IPC）--管道

进程间的通信（IPC）--管道

1.进程间通信常用的方式 1 &#xff0c;管道通信&#xff1a;有名管道&#xff0c;无名管道 2 &#xff0c;信号 - 系统开销小 3 &#xff0c;消息队列 - 内核的链表 4 &#xff0c;信号量 - 计数器 5 &#xff0c;共享内存 6 &#xff0c;内存映射 7 &#xff0c;套接…
阅读更多...
人称“灯爷”的灯光师到底要做些什么，看看他的岗位说明书

人称“灯爷”的灯光师到底要做些什么，看看他的岗位说明书

灯光师又称“灯爷”,是摄影制作部门负责灯光设备的技术人员,一般归摄影指导调配。被尊称“爷”,可见灯光师的地位不容小觑。那么这个岗位到底要做些什么呢&#xff1f; 岗位职责&#xff1a; 1、负责公司灯光设备的调制、维护和保养&#xff1b; 2、负责各包房灯光设备的调制、…
阅读更多...
Mac环境报错 error: symbol(s) not found for architecture x86_64

Mac环境报错 error: symbol(s) not found for architecture x86_64

Mac 环境Qt Creator报错 error: symbol(s) not found for architecture x86_64 错误信息 "symbol(s) not found for architecture x86_64" 通常是在编译或链接过程中出现的问题。这种错误提示通常涉及到符号未找到或者是因为编译器没有找到适当的库文件或函数定义。 …
阅读更多...
基于springboot+vue+uniapp的养老院系统小程序

基于springboot+vue+uniapp的养老院系统小程序

开发语言&#xff1a;Java框架&#xff1a;springbootuniappJDK版本&#xff1a;JDK1.8服务器&#xff1a;tomcat7数据库&#xff1a;mysql 5.7&#xff08;一定要5.7版本&#xff09;数据库工具&#xff1a;Navicat11开发软件&#xff1a;eclipse/myeclipse/ideaMaven包&#…
阅读更多...
初识git工具~~上传代码到gitee仓库的方法

初识git工具~~上传代码到gitee仓库的方法

目录 1.背景~~其安装 2.gitee介绍 2.1新建仓库 2.2进行相关配置 3.拉取仓库 4.服务器操作 4.1克隆操作 4.2查看本地仓库 4.3代码拖到本地仓库 4.4关于git三板斧介绍 4.4.1add操作 4.4.2commit操作 4.4.3push操作 5.一些其他说明 5.1.ignore说明 5.2git log命令 …
阅读更多...
ACC:Automatic ECN Tuning for High-Speed Datacenter Networks 相关知识点介绍（二）

ACC:Automatic ECN Tuning for High-Speed Datacenter Networks 相关知识点介绍（二）

目录 PerfTest工具 Incast traffic Incast Traffic 的原因 Incast Traffic 的影响 解决方法 流量负载 简单解释 影响因素 影响 管理方法 LINKPACK 主要特点 LinkPack 的应用 运行结果 Quantum ESPRESSO 主要特点 TensorFlow 主要特点 主要组件 Incast与qp …
阅读更多...
Ubuntu2023.04 浏览器不能上网的问题

Ubuntu2023.04 浏览器不能上网的问题

1.问题描述 ping www.baidu.com 是可以连接的&#xff0c;但是打开网页就是不能上网&#xff0c;但是自己查看了浏览器上面的设置&#xff0c;代理设置都是关闭的 再看了系统的设置代理也是关闭的&#xff0c;就是上不了网 解决方案&#xff1a; 455 echo $http_proxy456 e…
阅读更多...
JavaWeb项目中动态拼接sql语句

JavaWeb项目中动态拼接sql语句

业务需求描述&#xff1a; 图中的查询框在分条件查询用户信息列表时&#xff0c;前端可能会传回一个条件或多个条件&#xff0c;此时要对不同的条件进行sql语句的不同书写&#xff0c;前端传的情况有很多种&#xff0c;所以如果分情况写sql语句会比较死&#xff0c;并且不够灵活…
阅读更多...
机器学习之人脸识别-使用 scikit-learn 和人工神经网络进行高效人脸识别

机器学习之人脸识别-使用 scikit-learn 和人工神经网络进行高效人脸识别

文章摘要 本文将介绍如何使用 Python 的 scikit-learn 库和人工神经网络&#xff08;ANN&#xff09;来识别人脸。我们将使用 LFW 数据集&#xff08;Labeled Faces in the Wild&#xff09;&#xff0c;这是一个广泛用于人脸识别基准测试的大型人脸数据库。我们将展示如何准备…
阅读更多...
RedHat Enterprise Linux 7 YUM源（本地/网络源）配置详解

RedHat Enterprise Linux 7 YUM源（本地/网络源）配置详解

目录 一、挂载 二、建立本地源 三、建立网络源 四、验证可行性 一、挂载 ——将光盘挂载到 /mnt 下 当/mnt中有如图内容时&#xff0c;即挂载成功 若挂载光驱/dev/sr0时报错&#xff1a;mount: no medium found on /dev/sr0 解决措施&#xff1a;查看该设备状态是否全部勾选…
阅读更多...
数仓实践：一文读懂数仓 ODS 层模型设计

数仓实践：一文读懂数仓 ODS 层模型设计

引言 OneData 体系中，数据划分为三层： ODS（Operational Data Store）：操作数据层。它相当于数据中台通用数据模型层的一个数据准备区，同时又承担着基础数据的记录以及历史变化，主要完成业务系统、日志等结构化和半结构化数据引入到数据中台。保留业务系统原始数据，包括…
阅读更多...
【HZHY-AI300G智能盒试用连载体验】设置RKNN的开发环境

【HZHY-AI300G智能盒试用连载体验】设置RKNN的开发环境

目录 安装RKNN工具 安装pip3 安装RKNN Toolkit Lite2 安装RKNPU2运行库 本文首发于电子发烧友论坛&#xff1a;【新提醒】【HZHY-AI300G智能盒试用连载体验】 智能工业互联网网关 - 北京合众恒跃科技有限公司 - 电子技术论坛 - 广受欢迎的专业电子论坛! (elecfans.com) 前…
阅读更多...
C++学习/复习29--哈希

C++学习/复习29--哈希

一、 二、
阅读更多...
WordPress文章标题定制化前缀插件

WordPress文章标题定制化前缀插件

引言 在当今互联网的海洋中&#xff0c;吸引读者眼球的第一步往往始于文章标题的设计。对于WordPress博主而言&#xff0c;如何让每篇文章的标题更加个性化和吸引人&#xff0c;成为了一项重要的任务。传统的自定义CSS方法虽然可行&#xff0c;但其繁琐的操作和有限的美学效果…
阅读更多...
麦克斯韦方程组解析——电磁理论的基石与奥秘

麦克斯韦方程组解析——电磁理论的基石与奥秘

麦克斯韦方程组解析——电磁理论的基石与奥秘 麦克斯韦方程组的核心作用 组件/步骤描述麦克斯韦方程组描述电磁场的基本方程组&#xff0c;由四个主要方程构成功能揭示电场、磁场与电荷、电流之间的关系&#xff0c;是电磁理论的基础应用领域广泛应用于电子学、光学、通信等领…
阅读更多...
51单片机16（步进电机实验）

51单片机16（步进电机实验）

一、步进电机简介&#xff1a; 1、步进电机是将电脉冲信号转变为角位移或线位移的开环控制元件。 2、 3、 4、我们这个电机的旋转停止的位置只取决于脉冲信号的频率和脉冲数&#xff0c;而不受负载的变化的影响&#xff0c;也就是说给我们的这个步进电机一个脉冲信号&#x…
阅读更多...
大唐杯 5G LMT

大唐杯 5G LMT

一、比赛现场流程 比赛现场会给你一个册子&#xff0c;册子前边部分会告诉你要做什么&#xff0c;最后一页会给参数。 按照他告诉你要做什么一步步根据参数做就可以了。 他每组还会有个评分表&#xff0c;按照一步步的操作给你打分。 我们评分表这次是 基站登录—网络规划参…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023