ros2 c++实现JY_95T IMU解算三轴 加速度 角速度 欧拉角 磁力计 四元数
起因，机器人建图导航程序需要做里程计 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九轴模块使用说明：
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
#include
#include "serial/serial.h" //导入串口库，ros2不带串口库，需要单独安装
#include
#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
#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::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("/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 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();//创建对应节点的共享指针对象
rclcpp::spin(node); //运行节点，并检测退出信号
printf("线程退出\n");
rclcpp::shutdown();
return 0;
}
transform.hpp
//transform.hpp
#include
#include
#include
#include
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 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( 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
imu_get_cpp
0.0.0
TODO: Package description
m
TODO: License declaration
ament_cmake
rclcpp
std_msgs
serial
sensor_msgs
nav_msgs
tf2_ros
tf2_msgs
tf2_geometry_msgs
geometry_msgs
ament_lint_auto
ament_lint_common
ament_cmake
复制上面的代码到对应的文件/
编译：
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在转动，程序运行正常。
感谢大佬的程序分享，程序参考修改于：
【ROS2】获取imu数据并可视化保姆级教程(C++)_ros2 可视化_Glow_raw的博客-CSDN博客