一、机器人导航
1、背景：
现在很多酒店都配备了配送服务机器人，将物品放入机器人，然后指定房间号，机器人自己就能将物品送到对应的房间去，那么他是如何实现自主移动的呢？其实这样的移动机器人都是配备了导航功能，他知道自己当前所处环境的基础信息，也知道自己在该地图上的实时的位置信息，然后结合实时的传感器信息来规划路线，并进行导航。
这样来看机器人导航似乎是一个非常复杂的东西，但是结合ros2中优秀的开源框架，我们就可以快速的实现机器人的导航功能。
2、介绍：
假如你是一名仓库配货员，现在的任务是从A货架取某物品，那你面对两个问题：首先A货架在哪里，其次怎么走？对于移动机器人来说，要完成自主导航，其实就是要解决这两个问题，具体如何解决呢？接下来就讲解。
3、同步定位与地图构建：
想要导航，就要先要确定当前位置和目标点的位置，如何确定位置，就是定位问题。在导航软件导航中可以通过卫星定位确认机主当前所在的经纬度信息，也可以通过工具获取到，但对于在工厂里面工作的机器人来说，卫星定位肯定行不通的，因为有遮挡，这时我们就要通过其他传感器来获取位置信息了。
直到了当前位置和目标位置之后，想要规划处行走路线，还需要知道哪里是可以走的（比如说过道和高速公路等），哪里是不能走的（农田、大山......），此时就需要有一张标有环境的地图，在导航软件上这张图是有的，但对于工作在特定环境的机器人来说，就需要我们重新构建一张地图了，这就是为什么要构图。
前面我们学到了可以利用里程计可以获取机器人的位置信息，那我们利用激光雷达就可以获取环境的具体及深度信息，把他们结合起来，一边移动一边记录障碍物的信息，这样就可以实现定位和建图功能。
按以上假设，我们来实现下，打开上一章的项目，新终端rviz2
可以看到我们通过这种方式控制机器人前进后退、旋转，就可以把周围的环境信息大致描述出来，但是一旦我们使用旋转的时候，这个障碍物信息就会出现较大的偏差，所以通过以上演示可以看出来，如果只是简单的对数据进行叠加，会因传感器的速率同步和噪声问题导致出现错误的记录。因此，SLAM（Simultaneous Localization and Mapping，同步定位与地图构建）技术被提了出来，他就是结合特征提取和滤波等算法，来解决机器人定位和建图问题。
通过里程计加雷达SLAM就可以找到这过程中的噪声，然后把它去掉并把它结合成一张完整的地图。以上是原始的传感器数据进行SLAM，发现有很大的问题。
5、SLAM原理介绍：
机器人通过自身的传感器获取环境信息然后将这些信息进行处理后记录下来，这样就可以形成一张地图，根据常用传感器的类型不同，将SLAM分成激光SLAM和视觉SLAM。
激光SLAM主要使用激光雷达这些传感器获取环境的深度（即距离）信息，然后标记障碍物和自由空间进行建图和定位。视觉SLAM需要先使用相机等视觉传感器获取图像的信息，然后通过图像处理和特征提取来进行建图和定位。由于激光雷达可以直接获取环境深度信息，精度高且稳定，所以激光SLAM技术相对要成熟很多。
但要完成导航，只有地图还不够，还需要基于地图来进行路径规划并控制机器人移动到目标位置，这就是机器人导航要做的事。
6、机器人导航介绍：
解决了地图和定位问题后，我们就来考虑如何走的问题，第一步确认一条从当前位置到目标位置的路线，在以上地图中SLAM告诉我们机器人所在的位置，然后我们在机器人中选定一个目标点，然后我们的导航就可以通过路径规划算法找到一条从当前点到目标点的这样一条理想路径，理想中这条路径是要符合某些要求的，比如说既能避开所有的障碍物且又能耗时最少，要实现这种效果，我们就需要在地图上标记代价信息。因为我们要找到路径最短、耗时最短，你又能避开障碍物的，那我们要对地图进行特殊处理，就是添加代价信息。比如，如果我们要走路况差的路线，要花更多的时间，所以我们对路况不好的路线进行标记，确认路线的步骤就叫"全局路线规划"，确认一条起点到终点的全局路线，和我们导航规划的路线差不多的。
那在按照全局路线行走时，前方可能会出现一些障碍物，比如开车时发现前方有车道，是封闭的，此时就需要绕开维修车道，在机器人上想要绕过去，第一步要把障碍物添加到地图中，然后对当前的这一小段路径进行重新的路径规划，因为这个障碍物是小范围的，且动态的，所以我们往往会针对当前的小范围环境重新构建一张局部的构建地图，然后进行路径规划，确认局部路径这一步骤，我们就叫"局部路径规划"，考虑了全局路径和局部之后，有时我们还要面对一些特殊的情况，比如行走的过程中卡主或者是被行人遮挡，且找不到局部路径绕过去，此时，我们就需要一些相应的行为，帮助机器人脱离困境。比如说当前进卡主的时候，我们就尝试后退，当被人挡住的时候，就播放请让路这样的语言，对于这些遇到故障的问题的脱困行为，我们就称之为"恢复行为"，目前机器人导航系统，就是围绕着刚刚提到的这三件步骤，全局路径规划，局部路径规划，以及恢复行为，进行设计的。在对应的时机，分别调用全局路径规划、获取路径，根据当前的位置和全局路径再调用局部路径规划得到一小段路径后，控制机器人行走，当出现问题时调用恢复行为进行脱困。
二、slam_toolbox完成构图：
1、slam_toolbox介绍：
slam是通过传感器获取环境信息后进行定位和建图的，ros2提供了很多slam的功能包，比如说slam_toolbox、cartographer_ros、rtamap_slam等等，针对于二维的使用激光的场景，slam_toolbox开箱即用，上手较为简单，我们就用它来构建第一张地图。
slam_toolbox是一套用于2D SLAM的开源工具。
2、安装：
sudo apt install ros-$ROS_DISTRO-slam-toolbox
3、创建项目：
cp -r ../../chapt6/chapt6_ws/src/bot_description src/ 将上一章项目复制过来
4、建图：
先启动项目：ros2 launch bot_description gazebo_sim.lanuch.py
然后打开新终端：
ros2 launch slam_toolbox online_async_launch.py use_sim_time:=True
use_sim_time:=True：表示使用仿真时间；
5、开始构建并查看：
我们刚刚打开slam_toolbox 终端是不可以关闭的，接着我们在rviz中查看构图，注意这里要结合按键来一起执行。在rviz中，我们需要找到slam_toolbox的/map话题，打开它，并且打开RobotModel和TF，以方便等会运行机器人的时候看构图。
slam_toolbox会发布一个叫/map的话题，话题里面就是放的地图。
打开之后，等5S左右，会出现构图：
然后添加机器人模型：
自己记得打开TF。
再打开新终端，使用机器人控制节点：
ros2 run teleop_twist_keyboard teleop_twist_keyboard
控制机器人把整个房间都探索一遍，这样就可以构建一个完整的地图了。
灰色未知区域、白色无障碍空间，黑色表示障碍物、
slam有两个功能：一个是定位，一个是建图，建图就是上面那样。定位就是定出机器人在地图中的位置，所以就有机器人到map之间的坐标变换，通过TF可以获取。
slam_toolbox并没有直接发布从map到base_link之间的变换，发布的是map到odom之间，然后odom再到base_link之间这样的间接发布关系，可以使用rqt查看关系。
为了不打破原有的odom到base_footprint这样一个链式的结构，如果再发布到map到base_footprint，这样base_footprint就有两个父类了，就变成odom到base_footprint和map到base_footprint，这样在计算的时候就会相对于麻烦点，所以这里直接使用了从map到odom，odom到base_footprint这样一个间接发布。
6、保存地图：
1）介绍：
将地图保存为文件，一张地图可以用图片来表示，用了和真实的尺寸进行转换都会设置比例尺，导航地图也不例外，保存地图可以使用nav2_map_server这个工具，使用apt方便安装。
2）安装：
sudo apt install ros-$ROS_DISTRO-nav2-map-server
3）创建新功能包：
ros2 pkg create fishbot_navigation2
ros2 run nav2_map_server map_saver_cli -f room
生成了两个room文件，但是光有pgm是没办法知道尺寸的，所以我们有一个描述文件，room.yaml。pgm是一种图片格式，我们可以用potoshop等对图片进行修改。
mode: trinary------地图的类型，trinary表示地图中的每个像素点有三种可能的状态，障碍物的占据状态为黑丝，无障碍自由状态用白色表示，未知探索状态用灰色表示。
resolution：地图分辨率，0.05表示每个像素对应的物理尺寸为0.05M。
origin：坐标的原点；
negate：是否对地图进行取反，0表示不取反；
地图有三种状态：自由状态、占用状态和位置状态，occupied_thresh和free_thresh就是来区分状态的。假如把0-255映射到0-1之间，0-0.25为自由状态，>0.65占据状态，0.25-0.65之间就是未知状态。地图是基于传感器的观测，传感器其实是有噪声的，即使再好的传感器也有噪音也会有误差，所以某个地方到底是占据状态、自由物、还是障碍物并不能百分百的确定，于是就把整个地图划分成一个个小格子，某个格子被障碍物占据的概率与其像素值进行映射，所以这个地图还有另外一个名字，叫做占据栅格地图。
三、机器人导航
1、介绍navigation2：
navigation2是一个开源的机器人导航框架，他的目标是让机器人安全的从目标A点移动到B点，当然他并不是教你将机器人如何从A点搬运到B点，而是让机器人自主的进行移动，为了完成导航任务，路径规划，避障和自主脱困等能力便是navigation2所具备的基本功能。
在正式介绍navigation2之前先认识一个工具------行为树（Behavior Tree），行为树起源于游戏设计，它用于控制游戏的角色的行为（植物大战僵尸中，当僵尸出现时，豌豆射手就会开始射击），对于一个移动机器人来说，需要知道什么时候要进行路线规划，什么时候要自行脱困，和游戏中的角色行为相似，所以使用行为树来描述和管理机器人的行为非常合适，navigation2就是使用它来进行调度的，navigation2的系统框架，就像以上图所示。
首先是外面的圆角矩形，图中最大的矩形框是navigation2的核心部分，向内的箭头表示输入，向外的箭头表示输出，可以看到输入部分有TF变换、map、传感器数据（雷达、IMU等......）、行为树XMl（配置文件）、还有这些目标的位置。输出是一个（cmd_vel）机器人的基础控制器，cmd_vel就像我们常用的键盘控制节点的输出（机器人的角速度和线速度），所以navigation2我们假如把他当做一个黑盒，他就是通过TF坐标变换、map、传感器数据、行为树、然后给他一个目标点，他就可以输出机器人运动所需要的角速度和线速度，就可以控制机器人进行运动和避障，就这个结构。
内部结构：首当其冲就是一个叫BT的导航服务器，就是行为树（Behavior Tree）这样一个服务器，他的输入是一个行为树的XML行为树描述文件，通过这个描述文件会动态的调用下面三个模块，来进行这个导航控制。最右边的是规划器服务器------他的任务就是负责全局的路线规划，因为路径规划算法有很多，所以规划服务器可以根据配置加载不同的规划器来完成规划任务，所以加规划服务器，可以把规划器的插件插到这个规划服务器里面，所以他是规划服务器（不是单独的规划器，里面集成各种规划器）。规划服务器会输出一个全局的路径，还有一个全局的代价地图，并传递给控制器服务器（就是中间第二个模块），该模块负责根据全局路径规划输出的这样一个路径结合实时的障碍物和局部代价的地图完成机器人的控制。需要注意的是他同样也是一个服务器，他可以加载多钟不同的控制器进行完成控制这一任务。
恢复器服务器------机器人在运行了一半的过程中，发现障碍了，被困住了，此时就要调用恢复器服务器来进行恢复，恢复器服务器主要也是一个服务器，可以加载不同的恢复行为来完成机器人的脱困，如箭头可以看出，BT导航服务器收到目标点后会由规划器计算出一条从当前点到目标点的路径，然后再由这个控制器服务器进行路径的跟随，如果遇到卡主等困境的时候，则会调用恢复器完成脱困，就这样一个流程。整个流程的规划由BT导航服务器来完成。除了上图服务器外，navigation2还有添加一些辅助服务器，比如说路径平滑服务器等等。
这些服务器的协调工作就完成了整个navigation2的导航任务。
2、安装：
sudo apt install ros-$ROS_DISTRO-navigation2
为了方便使用navigation2，navigation2提供了示例功能包，我们一起安装
sudo apt install ros-$ROS_DISTRO-nav2-bringup 等上面安装完了，再安装这个
3、配置navigation2参数
我们把navigation2当做一个模块，只要给他输入正确的数据，他就可以正常的工作，所以在启动导航之前需要对一些参数进行调整以适配我们的仿真机器人。
这些参数主要有相关话题的名称，坐标器的名称和机器人描述等，nav2-bringup提供了一个默认的参数，我们只需要在这基础上进行修改即可。
获取默认参数配置：
\opt\ros\humble\share\nav2_bringup\params\nav2_params.yaml
复制到我们的目标文件下：
cp /opt/ros/humble/share/nav2_bringup/params/nav2_params.yaml .
参数名称中，带topic的基本都是有关话题的参数；带frame的，大多数表示TF坐标系的名称。而这些默认参数的名字跟我们上一章节创建仿真机器人使用插件生成的，比如激光雷达生成的这样的话题名字是一样的，所以话题和TF坐标系的名称就不用做修改了。在进行路径规划时，还需要考虑下机器人的大小，比如说半径这些参数，如果机器人的半径设置的比真实的大，就会遇到通过窄通道时，机器人过不去，如果过小，则会容易发生碰撞，由于只有在基于地图做路线规划时才会考虑问题，所以需要在全局代价地图global_costmap和局部代价地图local_costmap这两个里面修改机器人的半径robot_radius。我们暂时只需要确认节点名称以及修改robot_radius，其他的可以参考navigation2小鱼版文档：Nav2 --- Navigation 2 1.0.0 文档
4、编写launch并启动导航：
有了参数，接着我们就可以编写一个launch文件来传递参数，并启动导航。首先要新建一个launch文件，因为我们启动导航还需要传递一些参数给一个特定的launch。
1）编写launch：
2）安装：
3）启动：
启动前要先启动gazebo，因为导航前要有数据源。
ros2 launch bot_description gazebo_sim.lanuch.py
新终端启动导航：ros2 launch fishbot_navigation2 navigation2.launch.py
可以看到我们的地图加载过来了，但是看不到机器人的位置。
你也会发现global status打开后，里面是错误的，提示地图不存在，实际我们的地图是加载进来了的。在终端看到报错信息如下：
报一个TF错误，此时，我们打开rqt来看下：
可以看到odom到base_footprint到base_link有完整的这样的链条，整个这个TF是对的，但是我们的机器人导航的时候，机器人的坐标系是map，全局的global_frame_id，但整个TF tree里面没有map，我们在建图的时候是有map的，map到odom之间的坐标变换是有的，这个坐标变换是slam_toolbox发出来的，我们刚刚并没有启动slam_toolbox这个节点，因为我们的启动文件navigation2.launch.py默认是没有启动slam_toolbox的，理应当是有个节点来完成这个定位功能的，其实这个节点就在我们的配置文件nav2_params.yaml里，就叫amcl节点，这个节点就是负责定位的，负责根据输入的地图、还有里程计还有雷达这样的数据来估计机器人的位置，这个节点有个特定，就是在启动的时候需要给他一个大概的位置，就是你要告诉他机器人大概在哪个位置，然后他好在这个位置去做估计，所以在rviz里面有个2D Pose Estimate，就是给机器人一个预估的位置（注意机器人的朝向）， 就可以使用2D Pose Estimate来告诉amcl机器人大概的位置和朝向，你能告诉越精准越好，但是不精准也没事，只要在附近，朝向差不多，它在走的过程中会自动慢慢去校准的，在rviz中操作，记住旋转的过程中不要松开鼠标左键，确定好方向后松开鼠标，等待下，接下来就可以看到机器人的定位了。
可以看到有一层膜好像覆盖到地图上了，其实覆盖在rviz这些浅红色，在机器人周围的，就是全局的代价地图，而机器人那块颜色加深的那块就是局部代价地图，他们有什么作用呢？首先他们把障碍物（比如墙）给放大了，越深的颜色表示越危险，所以你看墙周边的颜色就是从墙到外越来越浅的。这里地图有两种，一种全局代价地图，一种局部代价地图，我们的navigation2框架中，控制器服务器里面是有个局部代价地图的，规划器服务器里面有全局代价地图，全局代价地图就是用于做全局规划的时候用的。
全局规划器
全局代价地图
有个控制器服务器，有个local costmap
5、进行单点与路点导航：
首先简单了解了地图的结构，我们就可以使用navigation2里面的这个nav2 Goal参数来给机器人设置目标点，让机器人自主的进行导航。使用rviz上的Nav2 Goal工具，然后随便选定一个目标点，并给个朝向。此时会有一条路径出现，这条路径就是全局规划出来的一条路径，机器人就可以按这个路径前进了。放大机器人的路径，可以看到一条蓝色的路径，这条路径就是我们的局部规划器所规划出来的局部路径，很短（是机器人以自身为半径一米左右的差误），这就是局部规划的作用，控制器服务器里面有局部代价地图，会基于局部代价地图做一个局部路径规划，他很短，只看周围的障碍物（比如你开车的时候，导航可能不知道某一块是否施工，是否有车占道，当你导航到哪个附近的时候，看到了，肯定就要重新规划下路线，不可能还按全局的走，继续全局走，可能就会撞车之类的）。
navigation2除了支持单点导航，还支持多点的导航，就是路点导航，路点就是指路过的点，navigation2就会在规划路径时，按照你指定的顺序进行导航。在rviz中选中waypoint/Nav Through Poses Mode，然后再使用Nav2 Goal分别打几个点，再点start Nav Through Pose，就可以看到机器人规划的路线了，依次走完所有的路径。
6、导航过程中进行动态避障：
会自动规避障碍物。
7、优化导航速度和膨胀半径：
在实际的机器人项目当中，因为各种的原因，比如说处于安全考虑，往往会对机器人的速度进行一些限制，我们可以通过修改机器人配置文件中机器人速度相关参数来限制机器人的最大速度，修改速度相关参数是在控制器服务器（controller_server）当中完成的，FollowPath------路径跟踪（执行路径的时候相关配置），这里使用的是dwb_core::DWBLocalPlanner进行的一个局部路径规划和跟踪，接下来是关于速度的配置，我们用最大旋转角速度（max_vel_theta）来举例,角速度的加速度配置（acc_lim_theta）。
重新colcon build，再运行。
经过膨胀之后，中间都没什么白色部分了，但是我们观察gazebo中，其实比机器人的路线大很多，这就说明我们对机器人中的膨胀半径这些参数调整的不是非常合理，膨胀半径其实是代价地图中的一个参数，一般设置为机器人的直径大小，我们可以修改下配置文件中对应的节点参数，所以我们可以修改local_costmap或者global_costmap中的膨胀半径的设置。
路径明显就变大了
8、优化机器人到点精准：
确切的来说，这并不是指到点精度，而是指到点的预值，比如说我们在启动导航的时候，随意制定一个目标点，我们把目标点指向某一个栅格的交界点，然后我们让机器人去导航，可以发现，最终机器人停在的位置并没有压着栅格的中心交叉点，是因为机器人导航到点的阈值设置的比较大，此时可以通过修改阈值来实现机器人到目标点这样的一个前后方向更加精准一些，我们知道负责机器人行走的这样组件是否到点了是控制器服务器，那到点精度、阈值、这个配置就在控制器服务器节点当中，找到general_goal_checker，可以看到他使用的也是一个插件，这里的插件其实就是行为树插件。同时修改三个参数：general_goal_checker下的xy_goal_tolerance以及yaw_goal_tolerance以及FollowPath下的xy_goal_tolerance
重新构建运行。
可以看到xy上的差距会比之前小些。