4.3雷达建图与导航
4.3.1启动激光雷达和数据查看
本节内容介绍激光雷达的启动和雷达数据的查看
机器人端启动激光雷达roslaunch robot_navigation lidar.launch
此时查看话题列表就会发现多了一个scan话题，这个就是雷达发布的。看到这个话题说明雷达已经正常启动。
通过查看scan话题就可以查看雷达数据
PC端运行rviz工具roslaunch robot_navigation lidar_rviz.launch
这里可以看到雷达数据，它通过激光扫面周围一圈。根据远近将障碍物以点的形式绘制出来。（雷达是单线的扫面范围只有雷达发射窗口所在高度的平面，高或者低于这个高度的物体扫描不到）
4.3.2机器人运行激光SLAM
这节内容演示激光雷达的SLAM建图功能
在机器人端执行slam的launch文件
roslaunch robot_navigation robot_slam_laser.launch
在PC端执行rviz图形化监控程序（载入rviz配置文件）
roslaunch robot_navigation slam_rviz.launch
雷达SLAM建图已经正常启动，rviz工具下可以看到以机器人为中心已经建立了一个初步的地图。接下来我们让机器人移动位置完善整个地图。
在PC端或机器人端启动一个键盘控制程序
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
这里我们根据提示按z键降低机器人移动速度（建议将遥控速度调低以改善建图效果，建议速度不大于0.25m/s注意，需要将鼠标光标保持在运行键盘控制程序的shell窗口内，否则相关按键无效 ）
地图中黑色边框部分就实际地形中的墙壁，在地图中是以边界的形式显示出来的。当地图四周边界已经全部闭合或者想要保存地图时候，在机器人端，新开一个控制终端(如果是在PC端执行下下述指令，则机器人端不会保存地图文件，下一章节导航会出现问题)，进入到想要保存地图的位置，然后保存地图，这里以robot_navigation功能包下maps为例（如果对这方面不熟悉建议按照下面命令行执行，因为后面的导航程序也是采用了这个路径和文件名称）
roscd robot_navigation/maps
rosrun map_server map_saver -f map
保存完成后maps文件夹下就会有map.pgm和map.yaml两个文件，到这里建图过程就全部完成了。
4.3.3运行激光雷达导航和避障
本节内容介绍使用激光雷达实现导航和避障的例程
在机器人端运行导航的launch文件
roslaunch robot_navigation robot_navigation.launch
（这里需要注意的是，机器人导航使用的是上一章节中建图后在默认路径下保存的默认地图文件，如果在上一章节中保存的地图名和路径是自定义的话是无法直接运行的，建议改成默认路径和文件名。）
在PC端执行rviz图形化监控程序
roslaunch robot_navigation navigation_rviz.launch
PC端启动之后rviz中显示如图，机器人当前实际位置和地图中的位置可能并不匹配，需要手动设置机器人位置。
点击图中红框内rviz上的”2D Pose Estimate”按钮，然后在地图上鼠标左键点中一个点，拉动鼠标制定方向，松开鼠标后刚点中的点和选择的方向即为机器人当前位置，对应的雷达数据也会更新过来，如后图
点击rviz上的”2D Nav Goal”按钮后，用鼠标在地图上左键点中一个点即可设为导航目标点，左键不放开时候拉动鼠标可以指定目标点方向，松开后机器人开始执行导航任务，在机器人运行launch文件的终端中也有相关的log输出。
此时机器人已经到达指定目标点。我们在它回程的路上放置一个障碍物（黑色方框部分），再让机器人导航回到刚才的位置。
此时观察机器人的路径规划就会发现，它已规划了一条避开障碍的路径，成功到达目标点
4.3.4激光SLAM建图算法切换
本节内容介绍SLAM建图算法的切换
SLAM建图有很多种算法，4.3.2节中我们演示了其中一种，我们再次启动一遍：
机器人端启动roslaunch robot_navigation robot_slam_laser.launch
此时我们查看已经启动的节点，它包含一个底盘控制节点base_control，激光雷达到底盘的坐标转换节点base_footprint_to_laser，激光雷达节点sc_mini等。其中gmapping就是机器人的建图算法节点。
现在我们尝试一下将建图算法gmapping更换成其他算法
机器人端启动roslaunch robot_navigation robot_slam_laser.launch slam_methods:=hector
使用hector可能会出现如下报错，这里无需理会。
这是由于我们使用一个launch文件来启动机器人和建图算法，然而launch文件是不保证各个节点的启动顺序的，hector启动速度比较快，底盘和雷达此时还未启动完成，所以会缺少odom信息，在底盘启动完成后报错信息会停止输出。
gmapping算法已替换成hector算法
在PC端执行rviz图形化监控程序
roslaunch robot_navigation navigation_rviz.launch
在机器人当中实现了有cartographer,hector, karto和gmapping四种常见的建图算法，在启动文件后传入上述的四种建图算法名称即可（缺省状态默认为gmapping）。如下：
roslaunch robot_navigation robot_slam_laser.launch slam_methods:=cartographer
roslaunch robot_navigation robot_slam_laser.launch slam_methods:=hector
roslaunch robot_navigation robot_slam_laser.launch slam_methods:=karto
4.3.5局部路径规划算法切换
本节内容介绍局部路径规划算法的切换
在4.3.3节中我们介绍了机器人的激光雷达导航是在机器人端运行导航的launch文件
roslaunch robot_navigation robot_navigation.launch
这个launch中局部路径规划默认使用的是TEB路径规划器
所谓局部路径规划相对的是全局路径规划，全局路径规划是机器人起点和目标点之间的完整的路径规划。因为机器人自身运动学特性的差异导致，它不是机器人可以立刻执行的路径。例如阿克曼结构的机器人无法原地转向等。这个时候就可以用到局部路径规划器来对全局路径规划进行动态的调整。
我们的机器人实现了DWA和TEB两种主流的路径规划器的使用，teb算法考虑到了机器人最小转向半径比较适合阿克曼结构的机器人，而dwa算法可以实现原地转向比较适用于差速转向车型。默认使用的是TEB路径规划器，如果需要切换为DWA路径规划器，只需要传入planner参数为dwa即可
roslaunch robot_navigation robot_navigation.launch planner:=dwa
4.3.6无地图场景导航并建图
本节内容演示机器人在无地图场景下导航并建图的应用
在机器人端执行slam的launch文件并传入planner参数为teb
roslaunch robot_navigation robot_slam_laser.launch planner:=teb
在PC端运行roslaunch robot_navigation slam_rviz.launch
参照4.3.3节雷达导航的方法点击rviz上的”2D Nav Goal”按钮给机器人设定一个目标点，此时机器人会在导航过程中不断完善地图。
导航和建图结束后保存地图的方法也是在机器人端保存即可
roscd robot_navigation/maps
rosrun map_server map_saver -f map
4.3.7stage机器人仿真器的使用
本节内容演示stage机器人仿真器的使用
在4.1.6节中讲解过机器人和PC端的分布式通讯，master运行在机器人上PC端通过机器人端运行的master与机器人进行通讯。而本章节的仿真实验是完全在PC下进行，不依赖实体机器人。所以需要将分布式通讯临时拆除，让master运行在PC上。
1、修改PC端~/.bashrc文件中的ROS_MASTER_URI值，修改.bashrc文件中的环境变量如下
#export ROS_MASTER_URI=http://192.168.31.118:11311
export ROS_MASTER_URI=http://`hostname -I | awk '{print $1}'`:11311
（即注释掉标号为1的那行，将标号为2的那行取消注释）
（在做实体机器人实验时需修改回来）
2、修改.bashrc中的BASE_TYPE环境变量为想要仿真的机器人类型（这个环境变量会影响到仿真中使用的机器人模型，这里以NanoCar为例）
export BASE_TYPE=NanoCar
修改完成后保存退出，source使之生效
source ~/.bashrc
3、接下来启动机器人的仿真
roslaunch robot_simulation simulation_one_robot.launch
启动后会弹出一个stage窗口），图中红色的小方块即为机器人的简化模型，黑色的即为机器人所处的环境模型。
这时再启动一个键盘遥控节点
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
可以通过键盘遥控机器人在环境中四处走动
Stage是可以模拟出机器人的碰撞效果的，如果你操控机器人碰撞到了墙壁，则会在碰撞位置出现一个黄色碰撞标志（如上图）。
4.3.8 stage中仿真建图导航
本节内容将会演示在stage仿真器中仿真机器人的激光雷达建图和导航
使用仿真功能可以避免做建图和导航算法开发时需要频繁的在实体机器人和真实环境中做测试，耗费大量时间，使用仿真可以专注于算法开发和参数优化，仿真中获得较好的效果时再将代码在真实机器人上测试，根据我们测试，使用我们的仿真环境中机器人的表现和实体机器人上的表现行为几乎一致。
在4.3.2章节中我们已经介绍了在实体机器人上实现激光雷达建图的应用，这一章节使用仿真来实现，使用的启动launch文件是一样的，不同的是这里需要通过launch文件传入配置参数来启用仿真
roslaunch robot_navigation robot_slam_laser.launch simulation:=true
simulation:=true使用仿真，设置为false则为使用实体机器人，参数缺省默认为false
如果在之前，没有解决好所使用的功能包的依赖关系，在运行后可能会出现类似这种报错提示，这是我们比较常见的报错。原因是缺包，只需手动安装即可，安装格式为sudo apt install ros-melodic-包名。例如：
sudo apt install ros-melodic-gmapping
安装完成后重新启动仿真建图
roslaunch robot_navigation robot_slam_laser.launch simulation:=true
启动一个rviz节点roslaunch robot_navigation slam_rviz.launch
这时再启动一个键盘遥控节点
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
启动完成后可以用和4.3.2章节一样的方法，通过键盘控制机器人遍历环境实现建图
直接在当前用户主目录下报存地图运行rosrun map_server map_saver -f stage_map
在运行仿真建图时还可以传入其他参数例如open_rviz为true，这样就会同时也打开rviz
roslaunch robot_navigation robot_slam_laser.launch simulation:=true open_rviz:=true
仿真环境实现机器人导航也是类似的方法
roslaunch robot_navigation robot_navigation.launch simulation:=true open_rviz:=true
启动之后的操作和4.3.3章节介绍的实体机器人操作完全一样，不同的地方在于3.5章节机器人是在真实的环境中运行，本章节机器人是在仿真环境中运行
需要注意的是PC上做仿真实验时环境变量中底盘类型BASE_TYPE设置为NanoCar或时需要将 robot_navigation/param/NanoCar/teb_local_planner_params.yam配置文件中的cmd_angle_instead_rotvel参数由false修改为true，这是由于stage仿真器中”Car”车型接受的cmd_vel话题中z轴角速度并非给定机器人角速度，而是指转向结构的转向角度，其他车型的仿真无需修改。
4.3.9 机器人多目标点导航
在4.3.3节中我们已经介绍了机器人的导航，它和多目标点导航的区别在于。普通的导航只能发布一个目标点，在抵达目标点前，如果发布了新的当目标点则会舍弃原本的目标点直接前往新的目标点。而多目标点导航则是按照目标点发布顺序依次前往。
在这里任然使用stage仿真实现
启动导航roslaunch robot_navigation robot_navigation.launch simulation:=true
如在实体机器人上运行则无需传入simulation:=true参数
启动多点导航工具roslaunch robot_navigation multi_points_navigation.launch
执行rviz图形化监控程序roslaunch robot_navigation multi_navigation.launch
可以看到，相比机器人导航章节的rviz界面中，多出了一个“2D Nav Goal”按钮，这两个按钮所触发的话题也是不同的，左侧的按钮和导航章节功能一样，给定机器人指定一个目标点，在机器人到达目标点之前指定新的目标点，则机器人会放弃之前的目标点而去新的目标点，右侧的按钮就是本章节的主角，可以连续给机器人指定多个目标点，机器人会依次去往各个目标点。
4.3.10 机器人多点全自动巡航
启动导航roslaunch robot_navigation robot_navigation.launch simulation:=true
如在实体机器人上运行则无需传入simulation:=true参数
执行rviz图形化监控程序roslaunch robot_navigation navigation_rviz.launch
然后在PC端新开一个终端订阅/move_base_simple/goal话题
rostopic echo /move_base_simple/goal
然后在rviz中通过“2D Nav Goal”按钮让机器人导航到你想要巡航的第一个点，这时订阅/move_base_simple/goal话题会输出一个话题
记录下红色框处的三个值
然后再导航到你想要导航到的第二个点，以此类推，记录下你想要到导航所有点后
打开robot_navigation功能包下的 way_point.launch文件进行修改
将刚刚记录下的值依次填入这个数组，这里要注意，每一纵列为一组，这里我设置了3个目标点（巡航点数量没有上限，但是至少要有2个点）。修改完成后保存退出
roslaunch robot_navigation way_point.launch loopTimes:=2
这里我们loopTimes 参数为巡航次数，默认为0次。设置为2则机器人在巡航点来回巡航两遍后停止，如果设置为0则一直巡航。
机器人在巡航中每到达一个巡航点会输出下一巡航点的目标ID，巡航次数到达后会输出Loop: 2 Times Finshed信息。