串口号后续我们需要使用，一般来说是返回/dev/ttyUSB0
2.安装ros2-serial
这个是串口字符数据转发到标准ROS2通信的库，安装步骤如下：
git clone https://github.com/RoverRobotics-forks/serial-ros2.git
cd serial-ros2
mkdir build
cd build
cmake ..
make
sudo make install
3.创建工作空间
先创建文件架，编译一个空的工作空间：
mkdir -p ~/imu_test_ws/src
cd ~/imu_test_ws
colcon build
再创建一个基于c++功能包，用于解析和读取imu数据：
ros2 pkg create --build-type ament_cmake imu_get
在imu_get功能包下的src文件夹里面创建下面三个文件后续在里面编写代码：
sudo gedit transform.hpp
sudo gedit publisher_imu.cpp
sudo gedit sub_imu.cpp
transform.hpp文件用于解析从imu受到的原始数据，publisher_imu.cpp用于发布imu数据的相关话题，sub_imu.cpp用于订阅话题。
接下来，我们就要编写三个文件里面的代码了。
4.编写transform.hpp
在coding之前，我们需要看一下imu的协议说明，大部分imu的协议都是大同小异的，只要能看明白一个型号的imu协议，其他型号也大差不差。我的imu协议说明如下：
这里以加速度为例，角度度和角度同理。一个完整的数据帧有11个字节，我们首先要读到协议头，以确保我们获取到一段完整的数据帧，然后识别数据类型，最后在根据协议给出的说明获取需要的数据，温度数据我这里没有用到，所以只对加速度数据的六个字节进行位运算。transform.hpp的代码如下：
#include <string>
#include <ctype.h>
#include <float.h>
#include <math.h>
class transform\_imu
{
private:
char \*stcTime;
char \*stcAcc;
char \*stcGyro;
char \*stcAngle;
public:
struct Acc
{
double x;
double y;
double z;
}acc{0,0,0};
struct Gyro
{
double x;
double y;
double z;
}gyro{0,0,0};
struct Angle
{
double r;
double p;
double y;
}angle{0,0,0};
/\*public:
transform\_imu(double,double,double);
//构造函数,也可尝试用初始化列表方式\*/
void FetchData(auto &data, int usLength)
{
int index = 0;
//char \*pData\_head = data;
//printf("count%x\n",\*pData\_head);
printf("%x\n",data[index]);
printf("count%d\n",usLength);
while (usLength >= 11)//一个完整数据帧11字节
{
printf("%x\n",data[index + 1]);
if (data[index] != 0x55)//0x55是协议头
{
index++;//指针(/索引)后移，继续找协议头
usLength--;
continue;
}
//for(int i = 0;i < 1000;i++){printf("once\n");}
if(data[index + 1] == 0x50) //time
{
stcTime = &data[index + 2];
//memcpy(&stcTime, &data[index + 2], 8);
}
else if(data[index + 1] == 0x51) //加速度
{
stcAcc = &data[index + 2];
//memcpy(&stcAcc, &pData\_head[index + 2], 8);
acc.x = ((short)((short)stcAcc[1]<<8 | stcAcc[0])) / 32768.00 \* 16 \* 9.8;
acc.y = ((short)((short)stcAcc[3]<<8 | stcAcc[2])) / 32768.00 \* 16 \* 9.8;
acc.z = ((short)((short)stcAcc[5]<<8 | stcAcc[4])) / 32768.00 \* 16 \* 9.8;
}
else if(data[index + 1] == 0x52)
{
stcGyro = &data[index + 2];
//memcpy(&stcGyro, &pData\_head[index + 2], 8);
gyro.x = ((short)((short)stcGyro[1]<<8 | stcGyro[0])) / 32768.00 \* 2000 / 180 \* M_PI;//弧度制
gyro.y = ((short)((short)stcGyro[3]<<8 | stcGyro[2])) / 32768.00 \* 2000 / 180 \* M_PI;
gyro.z = ((short)((short)stcGyro[5]<<8 | stcGyro[4])) / 32768.00 \* 2000 / 180 \* M_PI;
}
else if(data[index + 1] == 0x53)
{
stcAngle = &data[index + 2];
//memcpy(&stcAngle, &pData\_head[index + 2], 8);
angle.r = ((short)((short)stcAngle[1]<<8 | stcGyro[0])) / 32768.00 \* M_PI;
angle.p = ((short)((short)stcAngle[3]<<8 | stcGyro[2])) / 32768.00 \* M_PI;
angle.y = ((short)((short)stcAngle[5]<<8 | stcGyro[4])) / 32768.00 \* M_PI;
}
/\*case 0x54: //磁力计
memcpy(&stcMag, &pData\_head[2], 8);
mag.x = stcMag[0];
mag.y = stcMag[1];
mag.z = stcMag[2];\*/
//这里我一开始用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 <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 "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 <string>
#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<unsigned char> 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 <memory>
#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(<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})
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()
8.构建功能包运行并可视化
在imu_test_ws目录下执行下面命令：
colcon build --packages-select imu_get
构建成功后，我们需要对串口开放权限，再运行节点，终端输入下面命令：
最后的话
最近很多小伙伴找我要Linux学习资料，于是我翻箱倒柜，整理了一些优质资源，涵盖视频、电子书、PPT等共享给大家！
资料预览
给大家整理的视频资料：
给大家整理的电子书资料：
如果本文对你有帮助，欢迎点赞、收藏、转发给朋友，让我有持续创作的动力！
()
ament_package()
## 8.构建功能包运行并可视化
在imu\_test\_ws目录下执行下面命令：
colcon build --packages-select imu_get
构建成功后，我们需要对串口开放权限，再运行节点，终端输入下面命令：
### 最后的话
最近很多小伙伴找我要Linux学习资料，于是我翻箱倒柜，整理了一些优质资源，涵盖视频、电子书、PPT等共享给大家！
### 资料预览
给大家整理的视频资料：
[外链图片转存中...(img-iVQZoIJC-1727251757213)]
给大家整理的电子书资料：
[外链图片转存中...(img-HzEu8hHZ-1727251757214)]
**如果本文对你有帮助，欢迎点赞、收藏、转发给朋友，让我有持续创作的动力！**
扫一扫
944
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
