【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 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两种语言编写获取机器人姿态信息,并且结合前面的机器人运动编写了航向锁定节点,接下来会介绍机器人的更精彩的操作,期待你的关注。🎉🎉🎉

  • 1
    点赞
  • 9
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

2345VOR

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值