【7. ROS 中的 IMU 惯性测量单元消息包】

欢迎大家阅读2345VOR的博客【6. 激光雷达接入ROS】🥳🥳🥳
2345VOR鹏鹏主页： 已获得CSDN《嵌入式领域优质创作者》称号👻👻👻，座右铭：脚踏实地，仰望星空🛹🛹🛹
🎏🎏主要开发专栏🎏🎏
《arduino学习》：学习最简单开源便利的单片机Arduino，与时俱进😆😆😆
《Arduino编程参考》：本专栏围绕Arduino语法和Arduino库使用开发；🌻🌻🌻
《 Arduino小项目开发》：本专栏围绕Arduino生态结合实际需求设计综合的小项目开发。🌼🌼🌼
《HomeAssistant》：介绍homeassistant中基本开发， 重点设计esphome和nodered开发，包含小爱同学打印机等诸多设备添加。🎉🎉🎉
本文章属于《Ubuntu学习》和《ROS机器人学习》
：围绕Ubuntu系统基本配置及相关命令行学习记录！机器人操作系统 (ROS) 是一组软件库和工具，可帮助您构建机器人应用程序。👍👍👍

1. 前言

Ubuntu环境搭建
【经典Ubuntu20.04版本U盘安装双系统教程】
【Windows10安装或重装ubuntu18.04双系统教程】
【Ubuntu同步系统时间】
【Ubuntu中截图工具】
【Ubuntu安装QQ】
【Ubuntu安装后基本配置】
【Ubuntu启动菜单的默认项】
【ubuntu系统中修改hosts配置】
【18.04Ubuntu中解决无法识别显示屏】
【ROS 开发神器 Visual Studio Code 的安装和设置】
ROS学习笔记
【1. Ubuntu18.04安装ROS Melodic】
【2. 在Github上寻找安装ROS软件包】
【3. 初学ROS，年轻人的第一个Node节点】
【4. ROS的主要通讯方式：Topic话题与Message消息】
【5. ROS机器人的运动控制】
【6. 激光雷达接入ROS】
【7. ROS 中的 IMU 惯性测量单元消息包】
接下来学习ROS 中的 IMU 惯性测量单元消息包和导航，IMU 惯性测量单元是用来测量机器人的空间姿态！

2. IMU 惯性测量单元

2.1 sensor_msgs

进入ROS Index官网搜索sensor_msgs

进入website

2.2 IMU 惯性测量单元的格式定义

在消息中找到Imu

这就打开了IMU 惯性测量单元的格式定义

3. 使用C++实现IMU获取数据节点

3.1 通用IMU姿态数据格式

直接获取IMU融合好的机器人姿态四元数

3.2 构思功能的思路和步骤

构思

实现步骤

1. 构建一个新的软件包，包名叫做imu_pkg。
2. 在软件包中新建一个节点，节点名叫做imu_node。
3. 在节点中，向ROS大管家NodeHandle申请订阅话题/imu/data,并设置回调函数为IMUCallback()。
4. 构建回调函数IMUCallback()，用来接收和处理IMU数据。
5. 使用TF工具将四元数转换成欧拉角。
6. 调用ROS_INFO()显示转换后的欧拉角数值。

3.3 创建imu_pkg包

在工作空间src文件创建基于sensor_msgs模板的imu_pkg

cd ~/catkin_ws/src/
catkin_create_pkg imu_pkg roscpp rospy sensor_msgs

在imu_pkg文件夹下src中创建imu_node.cpp

3.4 编写imu_node订阅者节点

imu_node源码

#include <ros/ ros.h>
#include <sensor msgs/Imu.h>
#include "tf/tf.h"void IMUCallback(sensor msgs::Imu msg)
{if(msg.orientation_covariance[0] <0)return;tf::Quaternion quaternion(msg.orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w);double roll, pitch, yaw;tf::Matrix3x3(quaternion).getRPY( roll, pitch, yaw);roll = roll*180/M_PI;pitch = pitch*180/M_PI;yaw = yaw*180/M_PI;ROS_INFO(“滚转= %.0f 俯仰=%.0f 朝向= %.0f" , roll, pitch, yaw);
}
int main(int argc,char *argv[])
{setlocale(LC_ALL,"");ros::init(argc, argv,"imu_node" );ros::NodeHandle n;ros::Subscriber imu_sub = n.subscribe( "/imu/data" ,10,IMUCallback);ros::spin( );return 0;
}

ctrl+s快捷保存

3.5 设置C++编译规则

打开CMake文件

add_executable(imu_node src/imu_node.cpp)
add dependencies(imu_node ${${PROJECT_NAVE}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(imu_node${catkin_LIBRARIES}
)

ctrl+s快捷保存

ctrl+shift+b快捷编译

3.6 编译运行imu_node节点

编译，打开终端

cd ~/catkin_ws/
catkin_make

采用wpr_simulation开源工程，打开三个终端分别运行三条指令

roscore
roslaunch wpr_simulation wpb_simple.launch
rosrun imu_pkg imu_node

拖动机器人绕z轴正方向旋转90度

可参照可以打开wpr_simulation里src文件夹下的demo_imu_data.cpp文件，对照一下代码，排查错误

4. 用python获取IMU 数据节点

4.1 通用IMU姿态数据格式

直接获取IMU融合好的机器人姿态四元数

4.2 构思功能的思路和步骤

构思

实现步骤

1. 构建一个新的软件包，包名叫做imu_pkg。
2. 在软件包中新建一个节点，节点名叫做imu_node.py。
3. 在节点中，向ROS大管家rospy申请订阅话题/imu/data，并设置回调函数为imu_callback()。
4. 构建回调函数imu_callback()，用来接收和处理IMU数据。
5. 使用TF工具将四元数转换成欧拉角。
6. 调用loginfo()显示转换后的欧拉角数值。

4.3 创建imu_pkg包

在工作空间src文件创建基于sensor_msgs模板的imu_pkg，编译

cd ~/catkin_ws/src/
catkin_create_pkg imu_pkg roscpp rospy sensor_msgs
cd ..
catkin_make

在imu_pkg文件夹下新建script文件夹中创建imu_node.py

4.4 编写imu_node订阅者节点

先引入python包，设置中文utf-8显示

• ros>=20.04,采用python3
• ros<20.04,采用python

lidar_node.py源码

#!/usr/bin/env python3
#coding=utf-8
import rospy
from sensor msgs.msg import Imu
from tf.transformations import euler_from_quaternion
import mathdef imu callback(msg):if msg.orientation covariance[0]< 0:returnquaternion =[msg .orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w](roll,pitch , yaw) = euler_from_quaternion(quaternion)roll = roll*180/math.pipitch = pitch*180/math.piyaw = yaw*180/math.pirospy.loginfo(滚转=%.0f俯仰= %.0f朝向= %.of" ,roll,pitch,yaw)if _name ="_main_":rospy.init_node( "imu _node")imu_sub = rospy.subscriber( "/imu/data",Imu,imu_callback,queue_size=10)rospy.spin()

ctrl+s快捷保存

4.5 添加可执行的权限

在所在文件夹打开终端

cd catkin_ws/src/imu_pkg/scripts/
ls
chmod +x imu_node.py
ls

文件名变成绿色表示权限添加成功

4.6 运行imu_node节点

采用wpr_simulation开源工程，打开三个终端分别运行三条指令

roscore
roslaunch wpr_simulation wpb_simple.launch
rosrun vel_pkg vel_node.py

可参照可以打开wpr_simulation里的script文件夹中创建imu_node.py

5. 用C++编写 IMU 航向锁定节点

基于前面学习的机器人运动控制和 IMU 惯性测量单元数据，下面将联系这两点编写 IMU 航向锁定节点，我们可以直接在前面实验的程序上做修改

5.1 构思功能的思路和步骤

1. 让大管家NodeHandle 发布速度控制话题/cmd_vel。
2. 设定一个目标朝向角，当姿态信息中的朝向角和目标朝向角不一致时，控制机器人转向目标朝向角。

5.2 修改imu_node.cpp

见3.4源码
修改成如下lidar_node源码

#include <ros/ ros.h>
#include <sensor msgs/Imu.h>
#include "tf/tf.h"
#include "geometry msgs/Twist.h"ros::Publisher vel_pub;void IMUCallback(sensor msgs::Imu msg)
{if(msg.orientation_covariance[0] <0)return;tf::Quaternion quaternion(msg.orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w);double roll, pitch, yaw;tf::Matrix3x3(quaternion).getRPY( roll, pitch, yaw);roll = roll*180/M_PI;pitch = pitch*180/M_PI;yaw = yaw*180/M_PI;ROS_INFO(“滚转= %.0f 俯仰=%.0f 朝向= %.0f" , roll, pitch, yaw);double target_ yaw = 90;double diff_angle = target_yaw - yaw;geometry_msgS::Twist vel_cmd ;vel_cmd.angular.z = diff_angle * 0.0l;vel_pub.publish(vel_cmd):}
int main(int argc,char *argv[])
{setlocale(LC_ALL,"");ros::init(argc, argv,"imu_node" );ros::NodeHandle n;ros::Subscriber imu_sub = n.subscribe( "/imu/data" ,10,IMUCallback);vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);ros::spin( );return 0;
}

ctrl+s快捷保存

ctrl+shift+b快捷编译

可参照开源项目wpr_simulation下的src文件夹的demo_imu_behavior.cpp

5.4 运行imu_node节点

采用wpr_simulation开源工程，打开三个终端分别运行三条指令

roscore
roslaunch wpr_simulation wpb_simple.launch
rosrun imu_pkg imu_node

拖动机器人绕Z轴旋转，也会自动转回去

5.4 优化航向策略

当机器人检测前方障碍物时，最简单把转弯角度调大一点，原地转弯
imu_node源码

#include <ros/ ros.h>
#include <sensor msgs/Imu.h>
#include "tf/tf.h"
#include "geometry msgs/Twist.h"ros::Publisher vel_pub;void IMUCallback(sensor msgs::Imu msg)
{if(msg.orientation_covariance[0] <0)return;tf::Quaternion quaternion(msg.orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w);double roll, pitch, yaw;tf::Matrix3x3(quaternion).getRPY( roll, pitch, yaw);roll = roll*180/M_PI;pitch = pitch*180/M_PI;yaw = yaw*180/M_PI;ROS_INFO(“滚转= %.0f 俯仰=%.0f 朝向= %.0f" , roll, pitch, yaw);double target_ yaw = 90;double diff_angle = target_yaw - yaw;geometr_msgS::Twist vel_cmd ;vel_cmd.angular.z = diff_angle * 0.0l;vel_cmd.linear.x=0.1;vel_pub.publish(vel_cmd):}
int main(int argc,char *argv[])
{setlocale(LC_ALL,"");ros::init(argc, argv,"imu_node" );ros::NodeHandle n;ros::Subscriber imu_sub = n.subscribe( "/imu/data" ,10,IMUCallback);vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);ros::spin( );return 0;
}

ctrl+s快捷保存

ctrl+shift+b快捷编译
采用wpr_simulation开源工程，打开三个终端分别运行三条指令

roscore
roslaunch wpr_simulation wpb_simple.launch
rosrun imu_pkg imu_node

6. 用python编写 IMU 航向锁定节点

基于前面学习的机器人运动控制和 IMU 惯性测量单元数据，下面将联系这两点编写 IMU 航向锁定节点，我们可以直接在前面实验的程序上做修改

6.1 构思功能的思路和步骤

构思

实现步骤

1. 让大管家NodeHandle 发布速度控制话题/cmd_vel。
2. 设定一个目标朝向角，当姿态信息中的朝向角和目标朝向角不一致时，控制机器人转向目标朝向角。

6.2 修改imu_node.py

打开4.4编写imu_node.py

修改imu_node.py源码

#!/usr/bin/env python3
#coding=utf-8
import rospy
from sensor msgs.msg import Imu
from tf.transformations import euler_from_quaternion
import math
from geometry msgs.msg import Twistdef imu callback(msg):if msg.orientation covariance[0]< 0:returnquaternion =[msg .orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w](roll,pitch , yaw) = euler_from_quaternion(quaternion)roll = roll*180/math.pipitch = pitch*180/math.piyaw = yaw*180/math.pirospy.loginfo(滚转=%.0f俯仰= %.0f朝向= %.of" ,roll,pitch,yaw)target_yaw = 90diff_angle = target yaw - yawvel_cmd = Twist()vel_cmd.angular.z = diff_angle * 0.01global vel_pubvel_pub.publish(vel_cmd)if _name ="_main_":rospy.init_node( "imu _node")imu_sub = rospy.subscriber( "/imu/data",Imu,imu_callback,queue_size=10)vel_pub = rospy.Publisher( "/cmd_vel",Twist,queue_size=10)rospy.spin()

ctrl+s快捷保存

6.3 运行imu_node节点

采用wpr_simulation开源工程，打开三个终端分别运行三条指令

roscore
roslaunch wpr_simulation wpb_simple.launch
rosrun imu_pkg imu_node.py

6.4 优化航向策略

修改imu_node.py源码

#!/usr/bin/env python3
#coding=utf-8
import rospy
from sensor msgs.msg import Imu
from tf.transformations import euler_from_quaternion
import math
from geometry msgs.msg import Twistdef imu callback(msg):if msg.orientation covariance[0]< 0:returnquaternion =[msg .orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w](roll,pitch , yaw) = euler_from_quaternion(quaternion)roll = roll*180/math.pipitch = pitch*180/math.piyaw = yaw*180/math.pirospy.loginfo(滚转=%.0f俯仰= %.0f朝向= %.of" ,roll,pitch,yaw)target_yaw = 90diff_angle = target yaw - yawvel_cmd = Twist()vel_cmd.angular.z = diff_angle * 0.01vel_cmd.linear.x = 0.1global vel_pubvel_pub.publish(vel_cmd)if _name ="_main_":rospy.init_node( "imu _node")imu_sub = rospy.subscriber( "/imu/data",Imu,imu_callback,queue_size=10)vel_pub = rospy.Publisher( "/cmd_vel",Twist,queue_size=10)rospy.spin()

ctrl+s快捷保存
采用wpr_simulation开源工程，打开三个终端分别运行三条指令

roscore
roslaunch wpr_simulation wpb_simple.launch
rosrun imu_pkg imu_node.py

可参照可以打开wpr_simulation里的script文件夹中demo_imu_behavior.py

7. 总结

本节学习了ROS机器人的IMU 惯性测量单元，尝试C++和python两种语言编写获取机器人姿态信息，并且结合前面的机器人运动编写了航向锁定节点，接下来会介绍机器人的更精彩的操作，期待你的关注。🎉🎉🎉