咸宁公司网站建设,微网站管理平台,培训学校招生方案,网络公司seo教程欢迎大家阅读2345VOR的博客【6. 激光雷达接入ROS】#x1f973;#x1f973;#x1f973; 2345VOR鹏鹏主页#xff1a; 已获得CSDN《嵌入式领域优质创作者》称号#x1f47b;#x1f47b;#x1f47b;#xff0c;座右铭#xff1a;脚踏实地#xff0c;仰望星空#… 欢迎大家阅读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 构思功能的思路和步骤
构思
实现步骤
构建一个新的软件包包名叫做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.hvoid 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;
}
ctrls快捷保存
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}
)
ctrls快捷保存
ctrlshiftb快捷编译
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显示
ros20.04,采用python3ros20.04,采用python
lidar_node.py源码
#!/usr/bin/env python3
#codingutf-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_size10)rospy.spin()
ctrls快捷保存
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.hros::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.advertisegeometry_msgs::Twist(/cmd_vel,10);ros::spin( );return 0;
}ctrls快捷保存
ctrlshiftb快捷编译
可参照开源项目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.hros::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.x0.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.advertisegeometry_msgs::Twist(/cmd_vel,10);ros::spin( );return 0;
}
ctrls快捷保存
ctrlshiftb快捷编译 采用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
#codingutf-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_size10)vel_pub rospy.Publisher( /cmd_vel,Twist,queue_size10)rospy.spin()
ctrls快捷保存
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
#codingutf-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_size10)vel_pub rospy.Publisher( /cmd_vel,Twist,queue_size10)rospy.spin()
ctrls快捷保存 采用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两种语言编写获取机器人姿态信息并且结合前面的机器人运动编写了航向锁定节点接下来会介绍机器人的更精彩的操作期待你的关注。
