当前位置： 首页 > article >正文

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

article 2024/11/17 1:45:38

欢迎大家阅读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 math

def imu callback(msg):
	if msg.orientation covariance[0]< 0:
		return
	quaternion =[
		msg .orientation.x,
		msg.orientation.y,
		msg.orientation.z,
		msg.orientation.w
	]
	(roll,pitch , yaw) = euler_from_quaternion(quaternion)
	roll = roll*180/math.pi
	pitch = pitch*180/math.pi
	yaw = yaw*180/math.pi
	rospy.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 Twist

def imu callback(msg):
	if msg.orientation covariance[0]< 0:
		return
	quaternion =[
		msg .orientation.x,
		msg.orientation.y,
		msg.orientation.z,
		msg.orientation.w
	]
	(roll,pitch , yaw) = euler_from_quaternion(quaternion)
	roll = roll*180/math.pi
	pitch = pitch*180/math.pi
	yaw = yaw*180/math.pi
	rospy.loginfo(滚转=%.0f俯仰= %.0f朝向= %.of" ,roll,pitch,yaw)
	
	target_yaw = 90
	diff_angle = target yaw - yaw
	vel_cmd = Twist()
	vel_cmd.angular.z = diff_angle * 0.01
	global vel_pub
	vel_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 Twist

def imu callback(msg):
	if msg.orientation covariance[0]< 0:
		return
	quaternion =[
		msg .orientation.x,
		msg.orientation.y,
		msg.orientation.z,
		msg.orientation.w
	]
	(roll,pitch , yaw) = euler_from_quaternion(quaternion)
	roll = roll*180/math.pi
	pitch = pitch*180/math.pi
	yaw = yaw*180/math.pi
	rospy.loginfo(滚转=%.0f俯仰= %.0f朝向= %.of" ,roll,pitch,yaw)
	
	target_yaw = 90
	diff_angle = target yaw - yaw
	vel_cmd = Twist()
	vel_cmd.angular.z = diff_angle * 0.01
	vel_cmd.linear.x = 0.1
	global vel_pub
	vel_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两种语言编写获取机器人姿态信息，并且结合前面的机器人运动编写了航向锁定节点，接下来会介绍机器人的更精彩的操作，期待你的关注。🎉🎉🎉


http://www.kler.cn/a/15986.html

相关文章：

  • GitHub新手入门 - 从创建仓库到协作管理
  • HTML5实现俄罗斯方块小游戏
  • 深度解析 Feign
  • 【C++】深入理解自定义 list 容器中的 list_iterator：迭代器实现详解
  • go反射深入学习
  • 阅读2020-2023年《国外军用无人机装备技术发展综述》笔记_作战无人机和察打无人机图鉴
  • 有效日志管理在软件开发和运营中的作用
  • 某医院网络安全分析案例
  • 源码安装工具checkinstall使用
  • stable diffusion的使用
  • Windows10本地搭建网站教程 - 内网穿透发布公网访问
  • c#笔记-代码格式
  • 【MySQL】外连接查询
  • 浅谈一下布隆过滤器的设计之美
  • 【Python】实战：Python 实现前端、后端管理系统部署
  • RT-Thread GD32F4xx PWM设备驱动
  • python 离线安装pyinstaller
  • 国产ChatGPT大盘点
  • 10个必须掌握的SQL常用语句
  • Spring 管理 Bean-IOC--基于注解配置 bean
  • java实现乘法的方法
  • 在Docker上安装和运行MySQL容器（纯步骤）
  • 分部积分法习题
  • React 的源码与原理解读（九）：Lanes
  • alpine linux系统操作
  • 深度学习模型评估简单介绍