起因，机器人建图导航程序需要做里程计 imu数据融合，需要填充imu数据，但对imu填充的数据一直不是很了解，并且正在学习c++的类与对象，新近入手了一款JY_95T IMU，没有ros2的c++实现，所幸就拿它练练手，用c++实现读取imu数据。
JY_95T IMU 解算数据的python实现：
ros2小车串口安装GY_95T IMU陀螺仪加速度计初体验_JT_BOT的博客-CSDN博客
配置环境参考：
怎样愉快的连接使用usb转串口设备_JT_BOT的博客-CSDN博客
串口读取原理参考：
怎样愉快的使用串口发送16进制数据并读取串口内容_JT_BOT的博客-CSDN博客
JY_95T IMU九轴模块使用说明：
扫描二维码关注公众号，回复：
15660555 查看本文章
imu传感器数据解算原理：
此款imu内置44个寄存器，每个寄存器存储一个字节的数据，对应于不同的功能，具体功能参考上面的使用说明，其中8-42号寄存器储存的是imu的3个轴的实时数据，我们的目的就是通过c++读取这35个寄存器数据并加以解算。从串口读取到的一个完整的数据帧包含40位数据，0-3位4个Byte是imu的配置信息，4-38位35个Byte是数据信息，39位是校验和低八位。下面的程序就围绕这个原理展开。
运行环境：ubuntu 20.04 ros2 foxy
需要提前安装好ros2 ，不会安装的参考：
从零开始安装机器人foxy系统_foxy安装_JT_BOT的博客-CSDN博客
创建ros2工作空间：
mkdir -p ros2_ws/src //创建ros2工作空间
cd ros2_ws/src
ros2 pkg create imu_get_cpp --build-type ament_cmake --dependencies rclcpp //创建功能包
cd imu_get_cpp/src
touch publisher_imu.cpp //生成文件
touch transform.hpp //生成文件
cd ~/ros2_ws //返回工作空间
colcon build //编译
文件目录结构：
publisher_imu.cpp
//publisher_imu.cpp
#include <chrono>
#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 "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 <string>
#include "rclcpp/rclcpp.hpp"
#include "transform.hpp"
serial::Serial ser;
using namespace std::chrono_literals;
class publisher_imu_node : public rclcpp::Node// 创建imu发布类继承自rclcpp::Node
{
private:
std::string port; //端口名
int baudrate; //波特率查看硬件说明
transform_imu imu_fetch; //创建imu获取对象
rclcpp::TimerBase::SharedPtr timer_; //创建定时器
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu; // 创建发布者
public:
publisher_imu_node()
: Node("publisher_imu_node") //构造函数
{
port = "/dev/ttyUSB0"; // 根据自己的主机修改串口号
baudrate = 115200; // 波特率，imu在波特率115200返回数据时间大概是1ms,9600下大概是10ms
try
{
ser.setPort(port); //设置端口
ser.setBaudrate(baudrate); //设置波特率
serial::Timeout to = serial::Timeout::simpleTimeout(1); //设置超时
ser.setTimeout(to);
ser.open(); //打开串口
}
catch (serial::IOException &e)
{
RCLCPP_INFO(this->get_logger(), "不能打开端口 ");
return;
}
if (ser.isOpen())
{
RCLCPP_INFO(this->get_logger(), "串口初始化完成！");
unsigned char cmd_buffer[5] ={0xA4,0x03,0x08,0x23,0xD2}; //JY-95T启用35个寄存器 需要发送的串口包命令
ser.write(cmd_buffer,5); //JY-95T可以根据命令选择工作方式
printf("发出命令: %x %x %x %x %x\n",cmd_buffer[0],cmd_buffer[1],cmd_buffer[2],cmd_buffer[3],cmd_buffer[4]);
}
else
{
RCLCPP_INFO(this->get_logger(), "Serial Port ???");
return;
}
pub_imu = this->create_publisher<sensor_msgs::msg::Imu>("/imu_data", 20);//创建了话题 /imu_data
// 创建定时器，100ms为周期，定时发布，这个时间设置超过200毫秒发布的数据卡顿
timer_ = this->create_wall_timer(100ms, std::bind(&publisher_imu_node::timer_callback, this));
printf("发布线程工作中\n");
}
public:
void timer_callback() //定时器的回调函数，每20毫秒回调一次
{
int count = ser.available(); // 读取到缓存区数据的字节数
if (count > 0)
{
int num;
rclcpp::Time now = this->get_clock()->now(); // 获取时间戳
std::vector<unsigned char> read_buf(count);//这里定义无符号，是因为read函数的形参是无符号
num = ser.read(&read_buf[0], count); // 读出缓存区缓存的数据，返回值为读到的数据字节数
imu_fetch.FetchData(read_buf, num);
sensor_msgs::msg::Imu imu_data;
//----------------imu data填充数据----------------
imu_data.header.stamp = now;
//imu_data.header.frame_id = "imu_link";
imu_data.header.frame_id = "map";
//加速度
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.gyro_x ;
imu_data.angular_velocity.y = imu_fetch.gyro_y ;
imu_data.angular_velocity.z = imu_fetch.gyro_z ;
//四元数
imu_data.orientation.x = imu_fetch.quat_Q0;
imu_data.orientation.y = imu_fetch.quat_Q1;
imu_data.orientation.z = imu_fetch.quat_Q2;
imu_data.orientation.w = imu_fetch.quat_Q3;
pub_imu->publish(imu_data); //向话题放数据
/*
Author: Mao Haiqing
Time: 2023.7.6
description: 读取IMU数据
*/
}
}
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<publisher_imu_node>();//创建对应节点的共享指针对象
rclcpp::spin(node); //运行节点，并检测退出信号
printf("线程退出\n");
rclcpp::shutdown();
return 0;
}
transform.hpp
//transform.hpp
#include <string>
#include <ctype.h>
#include <float.h>
#include <math.h>
class transform_imu
{
public:
double acc_x=0;//加速度
double acc_y=0;
double acc_z=0;
double gyro_x=0; //陀螺仪角速度
double gyro_y=0;
double gyro_z=0;
double angle_r=0; //欧拉角
double angle_p=0;
double angle_y=0;
double temp=0; //imu温度计
double mag_x=0; //磁力计
double mag_y=0;
double mag_z=0;
double quat_Q0=0; //四元数
double quat_Q1=0;
double quat_Q2=0;
double quat_Q3=0;
public:
transform_imu(){}; //默认构造函数
~transform_imu(){}; //析构函数
void FetchData(auto &data, int usLength) //获取数据
{
int index = 0;
unsigned char sum=0;
//for(int i = 0;i < usLength;i++){printf("%x ",data[i]);} //打印数据，查看收到的数据是否正确
while (usLength >= 40)//一个完整数据帧40字节[0xA4,0x03,0x08,0x23,数据35字节，校验位]
{
// 收到的数据开头4位：0xA4 帧头,0x03 读取,0x08 从8号寄存器开始,0x23 35个寄存器
if (data[index] != 0xA4)//0xA4是协议头
{
index++;//指针(/索引)后移，继续找协议帧头
usLength--;
continue;
}
//printf("找到数据包帧头，index是%d，uslength=%d\n",index,usLength);//每一次读取的数据长度
if(data[index + 1] != 0x03) //判断第二个数据是否正确
{
printf("第2数据包内容不对，进入下个循环");
usLength = usLength - 40;
index += 40;
continue;
}
// printf("读imu数据\n");
if(data[index + 2] != 0x08) //判断第3个数据是否正确
{
printf("第三数据包内容不对，进入下个循环");
usLength = usLength - 40;
index += 40;
continue;
}
// printf("第08寄存器是数据\n");
if(data[index + 3] != 0x23) //判断第4个数据是否正确
{
printf("第四数据包内容不对，进入下个循环\n");
usLength = usLength - 40;
index += 40;
continue;
}
//printf("35个数据包\n");
//0-39字节相加的值取低8位与第40位校验，参考GY-95T 九轴模块使用说明
for(int i=index;i<index+40;i++) //前39为求和
{
sum=0+data[i];
}
if(sum != data[index+39]) //判断前39个数据的和是否与第40位相等
{
printf("校验和不对，进入下个循环\n");
usLength = usLength - 40;
index += 40;
continue;
}
// printf("校验和通过，数据没有问题\n");
/*
根据模块使用说明可知，一个完整数据帧40字节[0xA4,0x03,0x08,0x23,数据35字节，校验位]
1.开头4位：0xA4 帧头,0x03 读取,0x08 数据位从8号寄存器开始,0x23 35个寄存器
2.加速度位：4-9
3.陀螺仪位：10-15
4.欧拉角：16-21
5.磁场校准：22
6.器件温度：23-24
5.磁场：25-30
6.四元数：31-38
数据解析一定注意2个字节拼在一起用short类型，然后在转换成double类型，总体加小括号在作除法，
否则short类型作除法会被截断降低精度，表现在图形显示界面会卡，数据不流畅。拼好的数据单位参考
使用手册。
*/
// 加速度
acc_x = ((double)((short)(data[index + 5]<<8 | data[index + 4])))/2048 * 9.8; //单位m/s^2
acc_y = ((double)((short)(data[index + 7]<<8 | data[index + 6])))/2048 * 9.8;
acc_z = ((double)((short)(data[index + 9]<<8 | data[index + 8])))/2048 * 9.8;
// 陀螺仪角速度
gyro_x = ((double)((short)(data[index + 11]<<8 | data[index + 10])))/16.4/57.3; //单位rad/sec 360/2*3.14=57.3
gyro_y = ((double)((short)(data[index + 13]<<8 | data[index + 12])))/16.4/57.3;
gyro_z = ((double)((short)(data[index + 15]<<8 | data[index + 14])))/16.4/57.3;
//欧拉角
//angle_r = ((double)((short)(data[index + 17]<<8 | data[index + 16])))/100 ;
//angle_p = ((double)((short)(data[index + 19]<<8 | data[index + 18])))/100 ;
//angle_y = ((double)((short)(data[index + 21]<<8 | data[index + 20])))/100 ;
//imu温度
temp = ((double)((short)(data[index + 24]<<8 | data[index + 23])))/100 ; //单位摄氏度
//磁力计
//mag_x = ((double)((short)(data[index + 26]<<8 | data[index + 25])));
//mag_y = ((double)((short)(data[index + 28]<<8 | data[index + 27])));
//mag_z = ((double)((short)(data[index + 30]<<8 | data[index + 29])));
//四元数
quat_Q0 = ((double)((short)(data[index + 32]<<8 | data[index + 31])))/10000 ; //无单位
quat_Q1 = ((double)((short)(data[index + 34]<<8 | data[index + 33])))/10000 ;
quat_Q2 = ((double)((short)(data[index + 36]<<8 | data[index + 35])))/10000 ;
quat_Q3 = ((double)((short)(data[index + 38]<<8 | data[index + 37])))/10000 ;
//JY-95T imu传感器9轴，数据解析完成，具体用什么数据可以根据实际需要选用
//调试结束后可以屏蔽以下printf
printf("加速度x y z: %lf %lf %lf\n",acc_x,acc_y,acc_z);
printf("角速度x y z: %lf %lf %lf\n",gyro_x,gyro_y,gyro_z);
//printf("欧拉角r p y: %lf %lf %lf %lf\n",angle_r,angle_p,angle_y);
printf("imu温度计 %lf\n",temp);
//printf("磁力计x y z: %lf %lf %lf\n",mag_x, mag_y,mag_z);
printf("四元数Q0 Q1 Q2 Q3: %lf %lf %lf %lf\n",quat_Q0,quat_Q1,quat_Q2,quat_Q3);
/*
Author: Mao Haiqing
Time: 2023.7.6
description: 读取IMU数据
*/
usLength = usLength - 40;
index += 40;
}
}
};
CMakeLists.txt
cmake_minimum_required(VERSION 3.5)
project(imu_get_cpp)
# 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(<dependency> 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})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
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>imu_get_cpp</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="[email protected]">m</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>serial</depend>
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>tf2_ros</depend>
<depend>tf2_msgs</depend>
<depend>tf2_geometry_msgs</depend>
<depend>geometry_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
复制上面的代码到对应的文件/
编译：
cd ~/ros2_ws //进入工作空间
colcon build --packages-select imu_get_cpp //编译
source ~/ros2_ws/install/setup.bash //source环境变量
ros2 run imu_get_cpp publisher_imu_node //运行节点输出
看到以上输出说明程序运行成功。
查看话题
ros2 topic list //查看话题
/clicked_point
/goal_pose
/imu_data
/initialpose
/parameter_events
/rosout
/tf
/tf_static
ros2 topic echo /imu_data //显示话题详细信息
header:
stamp:
sec: 1688645970
nanosec: 614702066
frame_id: map
orientation:
x: 1.3034
y: -0.2712
z: -0.1872
w: 0.9363
orientation_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
x: -314.4512195121951
y: 48.91463414634146
z: 0.0
angular_velocity_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
linear_acceleration:
x: -0.07177734375
y: -3.9046875
z: 9.168359375000001
linear_acceleration_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0rviz2可视化数据
安装rviz2可视化插件：
sudo apt install ros-$ROS_DISTRO-imu-tools
add By topic imu
可以看到图像随imu在转动，程序运行正常。
感谢大佬的程序分享，程序参考修改于：