6. 用C++获取ROS激光雷达数据节点
6.1 运行模板样机
采用wpr_simulation开源工程，打开三个终端分别运行三条指令
roscore roslaunch wpr_simulation wpb_simple.launch rosrunwpr_simulation deno_lidar_data
6.2 构思功能的思路和步骤
构思
实现步骤
- 构建一个新的软件包，包名叫做lidar_pkg。
- 在软件包中新建一个节点，节点名叫做lidar_node。
- 在节点中，向ROS大管家NodeHandle申请订阅话题/scan，并设置回调函数为LidarCallback()。
- 构建回调函数LidarCallback(),用来接收和处理雷达数据。
- 调用ROS_INFO()显示雷达检测到的前方障碍物距离。
6.3 创建lidar_pkg包
在工作空间src文件创建基于sensor_msgs模板的lidar_pkg
cd ~/catkin_ws/src/ catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs
在lidar_pkg文件夹下src中创建lidar_node.cpp
6.4 编写订阅者节点
lidar_node源码
#include <ros/ ros.h> #include <sensor msgs/Laserscan.h> void Lidarcallback(const sensor_msgs::LaserScan msg) { float fMidDist = msg.ranges[180] ; ROS_INFO("前方测距ranges [180]=%f 米", fMidDist); } int main(int argc,char *argv[]) { setlocale(LC_ALL, "" ); ros::init(argc, argv,"lidar_node" ); ros::NodeHandle n; ros::Subscriber lidar_sub = n.subscribe( " /scan", 10, &LidarCallback); ros::spin(); return 0; }
ctrl+s快捷保存
6.5 设置C++编译规则
打开CMake文件
add_executable(lidar_node src/lidar_node.cpp) target_link_libraries(lidar_node ${catkin_LIBRARIES} )
ctrl+s快捷保存
ctrl+shift+b快捷编译
6.6 编译运行lidar_node节点
编译，打开终端
cd ~/catkin_ws/ catkin_make
采用wpr_simulation开源工程，打开三个终端分别运行三条指令
roscore roslaunch wpr_simulation wpb_simple.launch rosrun lidar_pkg lidar_node
前方距离2.6m，然后在Gazebo中调整书柜，选择移动靠近机器人
可参照可以打开.wpr_simulation里的demo_lidar_data.cpp文件
7. 用python获取ROS激光雷达数据节点
7.1 运行模板样机
采用wpr_simulation开源工程，打开三个终端分别运行三条指令
roscore roslaunch wpr_simulation wpb_simple.launch rosrun wpr_simulation deno_lidar_data.py
7.2 构思功能的思路和步骤
构思
实现步骤
- 构建一个新的软件包，包名叫做lidar_pkg。
- 在软件包中新建一个节点，节点名叫做lidar_node.py。
- 在节点中，向ROS大管家rospy申请订阅话题/scan，并设置回调函数为LidarCallback()。
- 构建回调函数LidarCallback()，用来接收和处理雷达数据。
- 调用loginfo()显示雷达检测到的前方障碍物距离。
7.3 创建lidar_pkg包
在工作空间src文件创建基于sensor_msgs模板的lidar_pkg，编译
cd ~/catkin_ws/src/ catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs cd .. catkin_make
在lidar_pkg文件夹下新建script文件夹中创建lidar_node.py
7.4 编写订阅者节点
先引入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 LaserScan def Lidarcallback(msg): dist = msg.ranges [ 180] rospy.loginfo("前方测距 ranges [ 180] = %f 米" , dist) if _name ="_main_": rospy.init_node( "lidar_node" ) lidar_sub = rospy.Subscriber( " /scan" ,LaserScan,Lidarcallback,queue_size=10) rospy.spin()
ctrl+s快捷保存
7.5 添加可执行的权限
在所在文件夹打开终端
cd catkin_ws/src/lidar_pkg/scripts/ ls chmod +x lidar_node.py ls
文件名变成绿色表示权限添加成功
7.6 运行lidar_node节点
采用wpr_simulation开源工程，打开三个终端分别运行三条指令
roscore roslaunch wpr_simulation wpb_simple.launch rosrun lidar_pkg lidar_node.py
前方距离2.6m，然后在Gazebo中调整书柜，选择移动靠近机器人
可参照可以打开wpr_simulation里的script文件夹中创建lidar_node.py
8. 用C++编写激光雷达避障节点
基于前面学习的机器人运动控制和激光雷达数据，下面将联系这两点编写激光雷达避障节点
8.1 构思功能的思路和步骤
让大管家NodeHandle 发布速度控制话题/cmd_vel 。
构建速度控制消息包vel_cmd。
根据激光雷达的测距数值，实时调整机器人运动速度，避开障碍物。
8.2 修改lidar_node.cpp
见6.4源码
修改成如下lidar_node源码
#include <ros/ ros.h> #include <sensor msgs/Laserscan.h> #include <geometry msgs/Twist.h> ros::Publisher vel_pub; void Lidarcallback(const sensor_msgs::LaserScan msg) { float fMidDist = msg.ranges[180] ; ROS_INFO("前方测距ranges [180]=%f 米", fMidDist); geometry msgs::Twist vel_cmd ; if( fMidDist< 1.5) { vel_cmd.angular.z = 0.3; } else { vel_cmd.linear.x = 0.05; } vel_pub.publish(vel_cmd); } int main(int argc,char *argv[]) { setlocale(LC_ALL, "" ); ros::init(argc, argv,"lidar_node" ); ros::NodeHandle n; ros::Subscriber lidar_sub = n.subscribe( " /scan", 10, &LidarCallback); ros::spin(); return 0; }
ctrl+s快捷保存
ctrl+shift+b快捷编译
8.3 运行lidar_node节点
采用wpr_simulation开源工程，打开三个终端分别运行三条指令
roscore roslaunch wpr_simulation wpb_simple.launch rosrun lidar_pkg lidar_node
机器人撞到障碍物，机器人有宽度
8.4 优化避障策略
当机器人检测前方障碍物时，最简单把转弯角度调大一点，原地转弯
lidar_node源码
#include <ros/ ros.h> #include <sensor msgs/Laserscan.h> #include <geometry msgs/Twist.h> ros::Publisher vel_pub; int ncount = 0; void Lidarcallback(const sensor_msgs::LaserScan msg) { float fMidDist = msg.ranges[180] ; ROS_INFO("前方测距ranges [180]=%f 米", fMidDist); if(ncount > 0) { ncount--; return; } geometry msgs::Twist vel_cmd ; if( fMidDist< 1.5) { vel_cmd.angular.z = 0.3; ncount = 50; } else { vel_cmd.linear.x = 0.05; } vel_pub.publish(vel_cmd); } int main(int argc,char *argv[]) { setlocale(LC_ALL, "" ); ros::init(argc, argv,"lidar_node" ); ros::NodeHandle n; ros::Subscriber lidar_sub = n.subscribe( " /scan", 10, &LidarCallback); ros::spin(); return 0; }
ctrl+s快捷保存
ctrl+shift+b快捷编译
然后在调试就OK啦
可参照开源项目wpr_simulation下的src文件夹的demo_lidar_behavior.cpp
9. 用python编写激光雷达避障节点
9.1 构思功能的思路和步骤
构思
实现步骤
- 让大管家rospy 发布速度控制话题/cmd_vel 。
- 构建速度控制消息包vel_cmd。
- 根据激光雷达的测距数值，实时调整机器人运动速度，避开障碍物。
9.2 修改lidar_node.py
打开7.4编写lidar_node.py
lidar_node.py源码
#!/usr/bin/env python3 #coding=utf-8 import rospy from sensor_msgs.msg import LaserScan from geometry_msgs.msg import Twist count =0 def Lidarcallback(msg): global vel_pub global count dist = msg.ranges [ 180] rospy.loginfo("前方测距 ranges [ 180] = %f 米" , dist) if count > 0: count = count - 1 return vel_cmd = Twist() if dist< 1.5: vel_cmd .angular.z = 0.3 else: vel_cmd.linear.x = 0.05 vel_pub.publish(vel_cmd) if _name ="_main_": rospy.init_node( "lidar_node" ) lidar_sub = rospy.Subscriber( " /scan" ,LaserScan,Lidarcallback,queue_size=10) vel_pub = rospy.Publisher( " /cmd_vel" ,Twist , queue_size=10) rospy.spin()
ctrl+s快捷保存
9.3 运行lidar_node节点
采用wpr_simulation开源工程，打开三个终端分别运行三条指令
roscore roslaunch wpr_simulation wpb_simple.launch rosrun lidar_pkg lidar_node.py
可参照可以打开wpr_simulation里的script文件夹中demo_lidar_behavior.py
10. 总结
本节学习了ROS机器人的激光雷达原理和数据查看，尝试C++和python两种语言编写，并且结合前面的机器人运动编写了避障节点，接下来会介绍机器人的IMU传感器的操作。🎉🎉🎉
