ROS学习笔记（三）——调用激光雷达信息实现机器人避障
前言：
上文中提到的节点调用更像是一个消息的发布者，将自己的消息发布在一个话题中，此时需要有一个订阅者来阅读其中的信息为自己所用。
想象一个 “家庭聊天群”。
在这个群里：
这样，各个复杂的模块通过“发布/订阅”这种简单的方式就协同工作起来了！
- 发布者 就像是 在群里发言的家人。
- 订阅者 就像是 打开了群消息提醒的家人。
- 话题 就是这个 “家庭聊天群”本身，或者群里特定的讨论主题（比如“买菜信息”、“家庭新闻”）。
- 激光雷达 作为一个发布者，持续地向一个叫做
/scan的话题上发布“前方障碍物距离”数据。 - 路径规划算法 作为一个订阅者，订阅了
/scan话题。它一收到这些数据，就开始计算机器人应该如何行走，然后它自己又变成一个发布者，向另一个叫做/cmd_vel（控制命令）的话题发布“前进、转弯”的指令。 - 机器人的底盘电机 作为一个订阅者，订阅了
/cmd_vel话题。它一收到“前进、转弯”的指令，就立刻驱动轮子转动。
在机器人中的应用实例：
一个机器人上通常有很多传感器和执行机构。
本文将介绍如何通过订阅激光雷达发送的数据实现机器人的避障工作。
一、介绍物理模拟器Gazebo与可视化工具Rviz
Gazebo：机器人的“物理模拟器”
想象一下，你想测试一辆自动驾驶小车。在真实世界里测试，撞坏了可是很贵的！Gazebo 就是为你解决这个问题的。
- 它是什么？
Gazebo 是一个3D物理仿真环境。它不仅仅有图像，还严格地遵守物理定律，比如重力、摩擦力、碰撞、光线等等。 - 它做什么？
你可以在 Gazebo 里搭建一个虚拟世界（比如一个有家具的房间、一条马路），然后把你的机器人模型放进去。- 机器人可以在里面真正地“动起来”。
- 你可以给机器人发送“前进”指令，它会真的在虚拟世界里前进。
- 如果它撞到墙，Gazebo会模拟出碰撞效果，它会被挡住或弹开。
- 它甚至可以模拟机器人身上的传感器，比如激光雷达会返回虚拟的扫描数据，摄像头会看到虚拟世界的图像。
简单来说，Gazebo就是：
一个为机器人打造的、高度逼真的“虚拟现实游戏引擎”。 它负责“创造世界”和“执行物理规律”。
Rviz：机器人的“数据可视化器”
现在，你的机器人在 Gazebo 这个虚拟世界里跑起来了。但是，你怎么知道它“心里”是怎么想的？它“看”到了什么？它“计划”要去哪里？这时候就需要 Rviz 了。
- 它是什么？
Rviz 是一个3D数据可视化工具。它本身不模拟任何物理现象，它只是一个“显示器”或“仪表盘”。 - 它做什么？
它接收来自机器人程序（比如在 Gazebo 里运行的机器人）发出的各种数据，然后用直观的图形方式展示给你看。- 显示“所见”：如果机器人有激光雷达，Rviz 可以把扫描到的点云数据展示成一个轮廓图。
- 显示“所想”：如果机器人正在做路径规划，Rviz 可以把机器人计划要走的那条路线用一条彩色的线画出来。
- 显示“所在”：它可以显示机器人在地图中的实时位置和姿态。
- 显示“模型”：它可以显示机器人自身的3D模型，并让模型的关节随着真实数据运动。
简单来说，Rviz就是：
一个为机器人打造的“超级仪表盘”或“X光透视眼”。 它负责“把机器人的内部状态和感知数据，变得肉眼可见”。
二、使用Gazebo和Rviz
（若未安装wpr_simulation，可前往https://github.com/6-robot/wpr_simulation进行下载安装）
1、打开Rviz
打开终端：
roslaunch wpr_simulation wpb_simple.launch //打开Gazebo仿真 //打开一个新的终端 rviz2、配置环境
①如图
②点击ADD，在弹出的窗口中找到RobotModel,选中后点击OK
③重复②的步骤找到LaserScan
④按图示选择
⑤结果
左图中红色细线即为激光雷达检测到的数据。
三、创建获取激光雷达数据的功能包及节点
1、创建功能包和节点
（1）创建功能包
打开终端：
cd catkin_ws/src catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs(2)创建节点
打开VScode软件在~/catkin_ws/src/lidar_pkg/src目录下创建文件：lidar_node.cpp
编辑新建的节点文件，输入以下内容并保存。
#include<ros/ros.h> #include<sensor_msgs/LaserScan.h> //引入激光雷达数据的消息类型库，有了他，代码才能看懂激光雷达传来的数据 void LidarCallbake(const sensor_msgs::LaserScan msg)//定义一个回调函数LidarCallbake，雷达每次有新的数据，这个函数会自动执行 //sensor_msgs::LaserScan msg是激光雷达的数据 “包裹”，里面装着所有的距离信息。 { float fMidDist = msg.ranges[180]; //激光雷达会从不同角度测量距离，ranges是个 “距离数组”，里面存着各个角度的测量值。 //这里取第 180 个位置的距离，存到fMidDist这个变量里，通常这个角度对应的是正前方的测量距离。 ROS_INFO("前方测距 ranges[180] = %f米",fMidDist);//这行是在终端打印信息，把刚才取到的正前方距离fMidDist以 //“前方测距 ranges [180] = 具体数值 米” 的格式显示出来，方便我们查看。 } int main(int argc, char *argv[]) { setlocale(LC_ALL,""); //这行是设置字符编码，避免打印信息时出现中文乱码的情况，让中文能正常显示。 ros::init(argc, argv, "lidar_node");//这行是初始化 ROS 节点，给这个节点起了个名字叫lidar_node，这样 ROS 系统就能识别和管理这个节点了。 ros::NodeHandle nh;//启动大管家NodeHandle ros::Subscriber lidar_sub = nh.subscribe("/scan",10,&LidarCallbake);//订阅激光雷达的数据，订阅的话题是/scan，缓存长度为10.回调函数为LidarCallbake ros::spin();//这行是让程序进入 “循环等待” 状态，一直监听/scan话题，只要有新数据就执行LidarCallback函数，直到程序关闭。 return 0; }(3)设置CMakeLists.txt文件
在VScode中打开与节点在同一地址中的CMakeLists.txt文件：
将下方内容复制黏贴到末尾。
add_executable(lidar_node src/lidar_node.cpp) target_link_libraries(lidar_node ${catkin_LIBRARIES} )(4)编译
cd ~/catkin_ws/ //进入工作空间 catkin_make //编译2、运行新编写的lidar_node.cpp节点，查看订阅到的激光雷达数据信息
roslaunch wpr_simulation wpb_simple.launch //打开新的终端 rosrun lidar_pkg lidar_node结果如图：
也可在Gazebo中移动机器人或者障碍物的位置观察输出的距离。
四、将获取的激光雷达数据发布出去，给速度控制节点实现机器人的避障
（1）修改lidar_node节点
打开VScode软件在~/catkin_ws/src/lidar_pkg/src目录下的文件：lidar_node.cpp
修改其中内容（不懂处可以参考注释解释）：
#include<ros/ros.h> #include<sensor_msgs/LaserScan.h> //引入激光雷达数据的消息类型库，有了他，代码才能看懂激光雷达传来的数据 #include<geometry_msgs/Twist.h> //引入机器人运动控制的消息类型库，Twist 消息用于控制机器人的线速度和角速度 ros::Publisher vel_pub; //定义一个发布者vel_pub，作用是向机器人的运动控制话题发布运动指令。 int nCont = 0; void LidarCallbake(const sensor_msgs::LaserScan msg)//定义一个回调函数LidarCallbake，雷达每次有新的数据，这个函数会自动执行 //sensor_msgs::LaserScan msg是激光雷达的数据 “包裹”，里面装着所有的距离信息。 { float fMidDist = msg.ranges[180]; //激光雷达会从不同角度测量距离，ranges是个 “距离数组”，里面存着各个角度的测量值。 //这里取第 180 个位置的距离，存到fMidDist这个变量里，通常这个角度对应的是正前方的测量距离。 ROS_INFO("前方测距 ranges[180] = %f米",fMidDist);//这行是在终端打印信息，把刚才取到的正前方距离fMidDist以 //“前方测距 ranges [180] = 具体数值 米” 的格式显示出来，方便我们查看。 geometry_msgs::Twist vel_cmd; //创建一个Twist类型的变量vel_cmd，用来存储要发送的运动指令 if(nCont > 0) { nCont --; return; } if( fMidDist < 1.5) //这里是一个条件判断：如果正前方距离fMidDist小于 1.5 米，就执行大括号里的指令 { vel_cmd.angular.z = 0.5;//设置机器人的角速度angular.z为 0.5 nCont = 50; } else { vel_cmd.linear.x = 0.5;//设置机器人的线速度linear.x为 0.5 } vel_pub.publish(vel_cmd);//通过发布者vel_pub把存储了运动指令的vel_cmd发布出去 } int main(int argc, char *argv[]) { setlocale(LC_ALL,""); //这行是设置字符编码，避免打印信息时出现中文乱码的情况，让中文能正常显示。 ros::init(argc, argv, "lidar_node");//这行是初始化 ROS 节点，给这个节点起了个名字叫lidar_node，这样 ROS 系统就能识别和管理这个节点了。 ros::NodeHandle nh;//启动大管家NodeHandle ros::Subscriber lidar_sub = nh.subscribe("/scan",10,&LidarCallbake);//订阅激光雷达的数据，订阅的话题是/scan，缓存长度为10.回调函数为LidarCallbake ros::spin();//这行是让程序进入 “循环等待” 状态，一直监听/scan话题，只要有新数据就执行LidarCallback函数，直到程序关闭。 return 0; }（2）编译
cd ~/catkin_ws catkin_make(3)运行新节点（关闭之前打开的Gazebo 仿真）
roslaunch wpr_simulation wpb_simple.launch rosrun lidar_pkg lidar_node结果：机器人直行到距离前方障碍物1.5米时，绕Z轴转向，继续直行，绕开障碍物。
后续。，，，，，，
