1. 前言

Ubuntu环境搭建

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 构思功能的思路和步骤

构思

实现步骤

构建一个新的软件包，包名叫做imu_pkg。
在软件包中新建一个节点，节点名叫做imu_node。
在节点中，向ROS大管家NodeHandle申请订阅话题/imu/data,并设置回调函数为IMUCallback()。
构建回调函数IMUCallback()，用来接收和处理IMU数据。
使用TF工具将四元数转换成欧拉角。
调用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 构思功能的思路和步骤

构思

实现步骤

构建一个新的软件包，包名叫做imu_pkg。
在软件包中新建一个节点，节点名叫做imu_node.py。
在节点中，向ROS大管家rospy申请订阅话题/imu/data，并设置回调函数为imu_callback()。
构建回调函数imu_callback()，用来接收和处理IMU数据。
使用TF工具将四元数转换成欧拉角。
调用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 构思功能的思路和步骤

让大管家NodeHandle 发布速度控制话题/cmd_vel。

设定一个目标朝向角，当姿态信息中的朝向角和目标朝向角不一致时，控制机器人转向目标朝向角。

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.3 运行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 构思功能的思路和步骤

构思

实现步骤

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

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

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

1. 前言

2. IMU 惯性测量单元

2.1 sensor_msgs

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

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

3.1 通用IMU姿态数据格式

3.2 构思功能的思路和步骤

3.3 创建imu_pkg包

3.4 编写imu_node订阅者节点

3.5 设置C++编译规则

3.6 编译运行imu_node节点

4. 用python获取IMU 数据节点

4.1 通用IMU姿态数据格式

4.2 构思功能的思路和步骤

4.3 创建imu_pkg包

4.4 编写imu_node订阅者节点

4.5 添加可执行的权限

4.6 运行imu_node节点

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

5.1 构思功能的思路和步骤

5.2 修改imu_node.cpp

5.3 运行imu_node节点

5.4 优化航向策略

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

6.1 构思功能的思路和步骤

6.2 修改imu_node.py

6.3 运行imu_node节点

6.4 优化航向策略

7. 总结

热门文章

最新文章

相关课程

相关电子书

相关实验场景

推荐镜像

热门

活动广场

任务中心

开发者评测

高校计划

乘风者计划

训练营

阿里云MVP

话题

直播

下载

镜像站

技术资料

插件

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

1. 前言

2. IMU 惯性测量单元

2.1 sensor_msgs

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

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

3.1 通用IMU姿态数据格式

3.2 构思功能的思路和步骤

3.3 创建imu_pkg包

3.4 编写imu_node订阅者节点

3.5 设置C++编译规则

3.6 编译运行imu_node节点

4. 用python获取IMU 数据节点

4.1 通用IMU姿态数据格式

4.2 构思功能的思路和步骤

4.3 创建imu_pkg包

4.4 编写imu_node订阅者节点

4.5 添加可执行的权限

4.6 运行imu_node节点

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

5.1 构思功能的思路和步骤

5.2 修改imu_node.cpp

5.3 运行imu_node节点

5.4 优化航向策略

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

6.1 构思功能的思路和步骤

6.2 修改imu_node.py

6.3 运行imu_node节点

6.4 优化航向策略

7. 总结

热门文章

最新文章

相关课程

相关电子书

相关实验场景

推荐镜像