最全【ROS2】获取imu数据并可视化保姆级教程(C++)_imu可视化(2)，2024年最新【金九银十】

于 2024-05-07 16:36:32 发布
阅读量387 收藏 3
点赞数 4
分类专栏： 程序员 文章标签： c++ 开发语言
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/m0_57259945/article/details/138536943
版权

最后的话

最近很多小伙伴找我要Linux学习资料，于是我翻箱倒柜，整理了一些优质资源，涵盖视频、电子书、PPT等共享给大家！

资料预览

给大家整理的视频资料：

给大家整理的电子书资料：

如果本文对你有帮助，欢迎点赞、收藏、转发给朋友，让我有持续创作的动力！

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以点击这里获取！

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

    //这里我一开始用switch case的写法 
  /\*case 0x59: //四元数

memcpy(&stcQuat, &pData_head[2], 8);
quat.w = stcQuat[0] / 32768.00;
quat.x = stcQuat[1] / 32768.00;
quat.y = stcQuat[2] / 32768.00;
quat.z = stcQuat[3] / 32768.00;*/

        //这个型号imu传感器6轴，暂时不启用这些读取
        //default:printf("over\n");
    
    printf("over\n");
    usLength = usLength - 11;
    index += 11;

}

}

};


上面printf的内容是我调试用的，不需要可以注释掉。一开始我使用传地址的方式，传入函数的参数一个强制转换为(char\*)类型的数组名，内参是一个char指针变量，但发布节点运行到函数内解析数据的代码就挂掉了，无任何提示，所以我这里换了一种写法，后续无bug，但也没想白之前的写法有什么问题，有想法的bro可以交流以下。


ok，跳过题外话，我们接下来编写发布节点。


## 5.编写publisher\_imu.cpp


publisher\_imu.cpp的代码如下：

#include
#include <math.h>
#include “serial/serial.h” //前面安装的ROS2内置串口包
#include <memory.h>
#include “std_msgs/msg/string.hpp”
#include “sensor_msgs/msg/magnetic_field.hpp”
//#include “geometry_msgs/msg/detail/vector3__struct.hpp”
#include “sensor_msgs/msg/imu.hpp”
#include “tf2/LinearMath/Quaternion.h”
#include “tf2_ros/transform_broadcaster.h”
#include “tf2_ros/static_transform_broadcaster.h”
#include “nav_msgs/msg/odometry.hpp”
#include
#include “rclcpp/rclcpp.hpp”

#include “transform.hpp”

serial::Serial ser;

using namespace std::chrono_literals;

class publisher_imu_node : public rclcpp::Node// 节点命名与类最好一致
{
public:
std::string port;
int baudrate;
std::string imu_topic;
std::string imu_offline_topic;
transform_imu imu_fetch; // 初始值设为0
public:
publisher_imu_node()
: Node(“publisher_imu_node”)
{
int output_hz = 20; // 频率，看传感器说明书
// timer_ms = millisecond(output_hz);
std::chrono::milliseconds timer_ms{output_hz}; // 换算成ms
port = “/dev/ttyUSB0”; // 串口号，主机可查看
baudrate = 9600; // 波特率，imu在波特率115200返回数据时间大概是1ms,9600下大概是10ms，看imu参数设置
try
{
ser.setPort(port);
ser.setBaudrate(baudrate);
serial::Timeout to = serial::Timeout::simpleTimeout(500);
ser.setTimeout(to);
ser.open();
}
catch (serial::IOException &e)
{
RCLCPP_INFO(this->get_logger(), "Unable to open port ");
return;
}

    if (ser.isOpen())
    {
        RCLCPP\_INFO(this->get\_logger(), "Serial Port initialized");
    }
    else
    {
        RCLCPP\_INFO(this->get\_logger(), "Serial Port ???");
        return;
    }

    pub_imu = this->create\_publisher<sensor\_msgs::msg::Imu>("/imu\_data", 20);
    //<sensor\_msgs::msg::Imu>消息数据类型可自行查看对应文件，消息队列长度设20
    pub_imu_offline = this->create\_publisher<sensor\_msgs::msg::Imu>("/imu\_offline\_data", 20);
    // 这里创建了两个话题，一个是/imu\_data，一个是/imu\_offline\_data

    // imu = transform\_imu();//初始化对象
    // ser.flush();
    // int size;
    printf("Process working\_1\n");
    timer_ = this->create\_wall\_timer(500ms, std::bind(&publisher_imu_node::timer_callback, this));//std::chrono::milliseconds timer\_ms{output\_hz}; // 换算成ms
    printf("Process working\_2\n");   
}

public:
void timer_callback()
{
int count = ser.available(); // 读取到缓存区数据的字节数
if (count != 0)
{
// ROS_INFO_ONCE(“Data received from serial port.”);
int num;
rclcpp::Time now = this->get_clock()->now(); // 获取时间戳
std::vector read_buf(count);//这里定义无符号，是应为read函数的形参是无符号
//unsigned char read_buf[count]; // 开辟数据缓冲区，注意这里是无符号类型
num = ser.read(&read_buf[0], count); // 读出缓存区缓存的数据，返回值为读到的数据字节数

        std::vector<char> read\_buf\_char(count);//vector转char类型
        for(int i = 0;i < count;i++)
        {
            read_buf_char[i] = (char)read_buf[i];
        }
        imu_fetch.FetchData(read_buf_char, num);
        sensor_msgs::msg::Imu imu_data;         //
        sensor_msgs::msg::Imu imu_offline_data; //
        //------------------imu data----------------
        imu_data.header.stamp = now;
        imu_data.header.frame_id = "imu\_frame";

        imu_data.linear_acceleration.x = imu_fetch.acc.x;
        imu_data.linear_acceleration.y = imu_fetch.acc.y;
        imu_data.linear_acceleration.z = imu_fetch.acc.z;

        imu_data.angular_velocity.x = imu_fetch.angle.r \* 180.0 / M_PI;
        imu_data.angular_velocity.y = imu_fetch.angle.p \* 180.0 / M_PI;
        imu_data.angular_velocity.z = imu_fetch.angle.y \* 180.0 / M_PI;//数据结构里没有储存欧拉角的变量名称，用angular\_velocity.z凑合

        tf2::Quaternion curr_quater;
        curr_quater.setRPY(imu_fetch.angle.r, imu_fetch.angle.p, imu_fetch.angle.y);
        // 欧拉角换算四元数
        RCLCPP\_INFO(this->get\_logger(), "Publishing: '");
        //RCLCPP\_INFO(this->get\_logger(), "angle: x=%f, y=%f, z=%f",imu.angle.r, imu.angle.p, imu.angle.y);

        imu_data.orientation.x = curr_quater.x();
        imu_data.orientation.y = curr_quater.y();
        imu_data.orientation.z = curr_quater.z();
        imu_data.orientation.w = curr_quater.w();
        // RCLCPP\_INFO(this->get\_logger(), "Quaternion: x=%f, y=%f, z=%f, w=%f",
        // imu\_data.orientation.x, imu\_data.orientation.y, imu\_data.orientation.z, imu\_data.orientation.w);

        //---------------imu offline data--------------
        imu_offline_data.header.stamp = now;
        // imu\_offline\_data.header.frame\_id = imu\_frame;

        imu_offline_data.linear_acceleration.x = imu_fetch.acc.x;
        imu_offline_data.linear_acceleration.y = imu_fetch.acc.y;
        imu_offline_data.linear_acceleration.z = imu_fetch.acc.z;

        imu_offline_data.angular_velocity.x = imu_fetch.gyro.x;
        imu_offline_data.angular_velocity.y = imu_fetch.gyro.y;
        imu_offline_data.angular_velocity.z = imu_fetch.gyro.z;

        imu_offline_data.orientation.x = curr_quater.x();
        imu_offline_data.orientation.y = curr_quater.y();
        imu_offline_data.orientation.z = curr_quater.z();
        imu_offline_data.orientation.w = curr_quater.w();

        pub_imu->publish(imu_data);
        pub_imu_offline->publish(imu_offline_data); // 发布话题，往两个话题放数据
    }
}
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu_offline;

};

int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<publisher_imu_node>()); // 单线程，调用所有可触发的回调函数，将进入循环，不会返回
printf(“Process exit\n”);
rclcpp::shutdown();
return 0;
}


这里需要注意的是，**sensor\_msgs::msg::Imu**数据类型里面没有储存角度的变量名(如果没看错的话，大火可以自己点进去看看)，所以我把欧拉角放进**imu\_data.angular\_velocity**里面。


## 6.编写sub\_imu.cpp


写订阅话题的节点就比较简单，代码如下：

#include
#include “std_msgs/msg/string.hpp”
#include “sensor_msgs/msg/imu.hpp”
#include “sensor_msgs/msg/magnetic_field.hpp”

#include “tf2/LinearMath/Quaternion.h”
#include “tf2_ros/transform_broadcaster.h”
#include “tf2_ros/static_transform_broadcaster.h”
#include “nav_msgs/msg/odometry.hpp”
#include “rclcpp/rclcpp.hpp”
#include “std_msgs/msg/string.hpp”
using std::placeholders::_1;

class Imu_subscriber : public rclcpp::Node
{
public:
Imu_subscriber()
: Node(“sub_imu_node”)
{
subscription_1 = this->create_subscription<sensor_msgs::msg::Imu>(
“/imu_data”, 20, std::bind(&Imu_subscriber::topic_callback, this, _1));
subscription_2 = this->create_subscription<sensor_msgs::msg::Imu>(
“/imu_offline_data”, 20, std::bind(&Imu_subscriber::topic_callback, this, _1));
}//创建接收信息节点

private:
void topic_callback(const sensor_msgs::msg::Imu::SharedPtr imu_msg) const//注意上面有两次回调，所以打印出来/imu_data之后就是/imu_data_offline
{
RCLCPP_INFO(this->get_logger(), “加速度(x,y,z): ‘%lf’‘%lf’‘%lf’”, imu_msg->linear_acceleration.x,
imu_msg->linear_acceleration.y,imu_msg->linear_acceleration.z);
RCLCPP_INFO(this->get_logger(), “欧拉角(r,p,y): ‘%lf’‘%lf’‘%lf’”, imu_msg->angular_velocity.x,
imu_msg->angular_velocity.y,imu_msg->angular_velocity.z);//指针类型要用->
}
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subscription_1;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subscription_2;
};

int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Imu_subscriber>());
rclcpp::shutdown();
return 0;
}


## 7.CMakelist.txt和xml文件的配置


CMakelist.txt和package.xml文件的配置如下：

cmake_minimum_required(VERSION 3.5)
project(imu_get)

Default to C99

if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()

Default to C++14

if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()

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

find dependencies

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(serial REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
#find_package(geometry_msgs)
find_package(nav_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

uncomment the following section in order to fill in

further dependencies manually.

find_package( REQUIRED)

add_executable(publisher_imu_node src/publisher_imu.cpp src/transform.hpp)
ament_target_dependencies(publisher_imu_node rclcpp std_msgs)
ament_target_dependencies(publisher_imu_node rclcpp serial)
ament_target_dependencies(publisher_imu_node rclcpp sensor_msgs)
ament_target_dependencies(publisher_imu_node rclcpp rosidl_default_generators)
ament_target_dependencies(publisher_imu_node rclcpp nav_msgs)
ament_target_dependencies(publisher_imu_node rclcpp tf2_ros)
ament_target_dependencies(publisher_imu_node rclcpp tf2_msgs)
ament_target_dependencies(publisher_imu_node rclcpp tf2_geometry_msgs)

install(TARGETS
publisher_imu_node
DESTINATION lib/${PROJECT_NAME})

add_executable(sub_imu_node src/sub_imu.cpp)
ament_target_dependencies(sub_imu_node rclcpp std_msgs)
ament_target_dependencies(sub_imu_node rclcpp std_msgs)
ament_target_dependencies(sub_imu_node rclcpp serial)
ament_target_dependencies(sub_imu_node rclcpp sensor_msgs)
ament_target_dependencies(sub_imu_node rclcpp rosidl_default_generators)
ament_target_dependencies(sub_imu_node rclcpp nav_msgs)
ament_target_dependencies(sub_imu_node rclcpp tf2_ros)
ament_target_dependencies(sub_imu_node rclcpp tf2_msgs)
ament_target_dependencies(sub_imu_node rclcpp tf2_geometry_msgs)
install(TARGETS
sub_imu_node
DESTINATION lib/${PROJECT_NAME})

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

ament_package()


![在这里插入图片描述](https://img-blog.csdnimg.cn/8a2dfc44b89a458b8e92e76de9cf0fd7.png#pic_center)


## 8.构建功能包运行并可视化


在imu\_test\_ws目录下执行下面命令：

colcon build --packages-select imu_get


构建成功后，我们需要对串口开放权限，再运行节点，终端输入下面命令：

sudo chmod 777 /dev/ttyUSB0


**/dev/ttyUSB0**是我的串口号，前面有说明如何查串口号，自行修改就行，**每次运行都要授权串口**。


打开两个终端，分别输入下面命令运行节点

source install/setup.bash
ros2 run imu_get publisher_imu_node

source install/setup.bash
ros2 run imu_get sub_imu_node


通过订阅话题的形式，到这里就完成了imu数据的解析与获取，如果想看到更多的数据，可以在订阅节点多打印几个变量，或打开一个新的终端输入下面命令：

ros2 topic echo /imu_data

最后的话

最近很多小伙伴找我要Linux学习资料，于是我翻箱倒柜，整理了一些优质资源，涵盖视频、电子书、PPT等共享给大家！

资料预览

给大家整理的视频资料：

给大家整理的电子书资料：

如果本文对你有帮助，欢迎点赞、收藏、转发给朋友，让我有持续创作的动力！

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以点击这里获取！

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

PPT等共享给大家！

资料预览

给大家整理的视频资料：

[外链图片转存中…(img-0BrjOJXo-1715070979829)]

给大家整理的电子书资料：

[外链图片转存中…(img-ME21VGSn-1715070979830)]

如果本文对你有帮助，欢迎点赞、收藏、转发给朋友，让我有持续创作的动力！

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以点击这里获取！

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

v_648374
关注
  • 4
    点赞
  • 踩
  • 3
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
（四）可视化ＩＭＵ数据
rhyijg的博客
09-02 1万+
一、ＩＭＵ数据格式 从KITTI的readme文件中https://github.com/yanii/kitti-pcl/blob/master/KITTI_README.TXT可以看到IMU里数据的格式, 　　KITTI中的IMUS数据文件存在oxts文件夹下，以txt格式保存。 　　其中有[‘lat’,‘lon’,‘alt’,‘roll’,‘pith’,‘yaw’,‘vn’,‘ve’,‘vf’,‘vl’,‘vu’,‘ax’,‘ay’,‘az’,‘af’,‘al’,‘au’,‘wx’,‘wy’,‘wz’
node-imu-cube:用于加速度计和陀螺仪数据的浏览器内可视化工具
07-04
节点-imu-cube 浏览器中加速度计和陀螺仪数据的可视化工具
基于ARDUINO CLOUD的IMU采样及Processing可视化
unoleonardo的博客
11-14 1234
基于ARDUINO CLOUD的IMU采样及Processing可视化 干货满满！！！
ros imu可视化（ubantu）
m0_51977577的博客
05-09 418
ros imu可视化（ubantu）
【ROS2】获取imu数据并可视化保姆级教程(C++)_imu可视化
最新发布
2401_83946044的博客
04-19 736
void topic_callback(const sensor_msgs::msg::Imu::SharedPtr imu_msg) const//注意上面有两次回调，所以打印出来/imu_data之后就是/imu_data_offline。RCLCPP_INFO(this->get_logger(), “欧拉角(r,p,y): ‘%lf’‘%lf’‘%lf’”, imu_msg->angular_velocity.x,//指针类型要用->//unsigned char read_buf[count];
使用机器人操作系统ROS 2和仿真软件Gazebo 9命令遥控可视化教程（二）
01-06
摄像头图像显示和激光数据可视化 ROS2入门介绍参考：https://zhuanlan.zhihu.com/p/96940278 $ ros2 launch mobot world.launch.py 具体流程参考下图，配置不同会有路径和目录的差异： 节点 $ ros2 node usage: ...
基于ROS的IMU驱动，IMU数据的接收和解析
01-09
基于ROS的IMU驱动，两个cpp文件，分别是IMU数据的接收和解析
ros2下通过imu和odom里程计共同对激光雷达的运动畸变校正源码
09-08
ros2下通过imu和odom里程计共同对激光雷达的运动畸变校正源码 ros2下通过imu和odom里程计共同对激光雷达的运动畸变校正源码 ros2下通过imu和odom里程计共同对激光雷达的运动畸变校正源码 ros2下通过imu和odom里程计...
ign-rviz：使用点火库的ROS 2可视化
01-28
是用于使用ROS的机器人的3D可视化工具。 Ignition RViz提供与RViz类似的功能，并且是使用库开发的。 建立状态 ROS2发行版 状态 狡猾 滚动 转至以获取该项目的详细描述。 要求 （开发中） 3 4 6 4 GPS插件所...
可视化IMU传感器的3D运动-项目开发
03-30
在本教程中，您将学习如何以3D形式显示IMU（惯性测量单元）传感器“ MPU-6050 [6DOF]”的运动。
IMU数据采集与处理
04-26
与惯性测量单元的数据采集和处理相关.基于LINUX的环境。
【ROS2】获取imu数据并可视化保姆级教程(C++)
Vingnir的博客
04-03 5591
本文采用的是六轴imu，但九轴或十轴的imu方法类似，系统是ubuntu20，ros2是foxy版本。
ROS2中IMU话题的发布及可视化
随便写写
03-09 1556
代码是从维特智能的示例代码修改的，实现基本的加速度、角速度和角度读取，发布IMU消息。这个传感器还支持磁场输出等功能，后面再加上吧。安装用于IMU消息显示的插件，之后在 Add -> By topic 中选择IMU消息即可。环境：Ubuntu 20.04，ROS2 Foxy。传感器：维特智能BWT901CL。RViz2进行可视化，需用。
ROS：常用可视化工具的使用
立于皓月之边，不弱星光之势
06-05 923
ROS：常用可视化工具的使用
ROS学习（6）可视化和调试工具二
韩若熙的博客
03-17 2668
ROS自带了大量功能强大的工具，其中包括消息日志系统、诊断消息、可视化及检测工具。该篇为上一篇的延续。内容较多，在以后的学习中熟练运用就好。httpshttps。
【ros】结果实时在线可视化
感知算法
04-10 878
1. 感知与规划控制是无人驾驶算法重要算法，在交付测试阶段也最容易引起摩擦，这也是司空见惯的现象。有时候可能是接口对齐问题、有时候可能是版本管理问题、有时候可能是第三方如云平台问题、有时候车子硬件自身问题。 2. 当然有时候也可能是**感知算法**与**规控算法**自身问题，所以拥有"自证清白"的能力是十分必要的。比如今天博主分享的感知结果实时可视化。具体实现可见下文。 3. ros 订阅 perception 结果进行在线可视化。发布 sensor_msgs::Image 结果，通过 rviz 实时在线观
将IMU数据可视化（画图）python实现
qq_36267302的博客
10-20 1674
IMU数据作图（python实现）
ros2 cartographer imu
08-06
回答: 在ROS2中使用cartographer进行imu配置时，可以参考鱼香ROS提供的教程。根据引用中的信息，虽然在ROS2中无法直接安装cartographer包，但是可以安装cartographer_ros包，它依赖于cartographer。所以，通过安装...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值