[feat]:第十章迁移

docs/humble/chapt10/advanced/1.ROS2地图加载与编辑.md

@@ -0,0 +1,181 @@
+# 1.地图文件介绍
+
+## 1.地图数据文件.pgm介绍
+
+OccupancyGrid由一个`.yaml格式`的元数据文件，和.pgm图片格式的地图数据文件组成。从上节课建图后保存的文件也可以看出。
+
+```
+.
+├── fishbot_map.pgm
+└── fishbot_map.yaml
+
+0 directories, 2 files
+```
+
+
+
+![image-20220514115948139](1.ROS2地图加载与编辑/imgs/image-20220514115948139.png)
+
+打开上节课建好的地图，观察下，你应该会有几点疑问：
+
+1. 不是说占据栅格地图每个栅格都有一个概率吗？为什么看不出来？
+2. 立方体内和圆柱体内和墙之外的区域为什么是灰色的？
+
+原因如下：
+
+1. 一个栅格对应到图片上其实是一个像素，每一个像素的值在0-255之间，所以将像素值和占据率之间的映射即可，而像素值反应到图像上就是颜色的深浅，1.2图对应的像素颜色如下：
+
+![image-20220506141320374](1.ROS2地图加载与编辑/imgs/image-20220506141320374.png)
+
+2. 建图的时候物体内和墙之外的区域机器人并没有探索到，没有数据参考就认为其值是未知的。
+
+## 2.地图描述文件.yaml介绍
+
+除了`fishbot_map.pgm`文件外，还有另外一个`fishbot_map.yaml`的文件，`fishbot_map.yaml`文件是地图的配置文件，该文件内容如下：
+
+```yaml
+image: fishbot_map.pgm
+mode: trinary
+resolution: 0.05
+origin: [-3.37, -2.88, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25
+```
+
+- image：图像名称
+
+- mode：图像模式，默认为trinary(三进制)，还有另外两个可选项scale(缩放的)和raw(原本的值)。
+
+- resolution：分辨率，一个栅格对应的物理尺寸，单位为m。0.05则表示一个栅格为0.05m
+
+- origin：地图原点位置，单位是m。
+
+
+- negate：是否反转图像
+
+- cooupied_thresh：占据阈值
+
+- free_thresh：空闲阈值
+
+#### 如何在地图上找出机器人原点的像素位置？
+
+  1. 图像的像素原点在左下角
+
+  2. 借张图左边的0,0是地图原点，右边的图是图像的像素原点
+
+     ![img](1.ROS2地图加载与编辑/imgs/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3o4MjQwNzQ5ODl5,size_16,color_FFFFFF,t_70.png)
+
+3. 假如机器人地图原点是[-3.37m, -2.88m]，除上分辨率则可得到像素原点[-67.4px,-57.6px]。`地图原点`在x和y轴分别偏移[-67.4px,-57.6px]就到了像素原点，反过来说，地图原点就在像素原点的[67.4px,57.6px]处。
+
+## 3.地图加载
+
+### 3.1 启动地图
+
+了解完地图的格式和描述文件，接着我们看看如何将地图加载到rviz2中进行显示。我们使用nav2_map_server进行地图的保存，也可以使用它来加载地图。
+
+打开终端，进入`src/map/`运行下面指令：
+
+```shell
+ros2 run nav2_map_server map_server --ros-args --param yaml_filename:=map/fishbot_map.yaml
+```
+
+其中`--ros-args --param yaml_filename:=map/fishbot_map.yaml`指定地图描述文件名称`.fishbot_map.yaml`。
+
+### 3.2 打开rviz2
+
+打开终端
+
+```
+rviz2
+```
+
+通过add->bytopic->选择map组件。此时你应该什么都看不到，没关系，这是正常的。
+
+### 3.3 配置并激活map_server
+
+`map_server`节点采用的是ROS2的生命周期进行编写的，除了启动节点，我们还需要手动的配置和激活节点才能使用。
+
+打开新的终端，使用下面指令进行配置：
+
+```
+ros2 lifecycle set /map_server configure
+```
+
+使用下面的指令进行激活：
+
+```
+ros2 lifecycle set /map_server activate
+```
+
+完成这一步，你应该就可以在`rviz2`中看到地图了。
+
+
+
+![image-20220514151400592](1.ROS2地图加载与编辑/imgs/image-20220514151400592.png)
+
+
+
+将栅格数量改大，然后将Grid的cellsize改成0.05,你就可以看到我们之间建立的地图每一个栅格的占据情况了。
+
+
+
+![image-20220514193704671](1.ROS2地图加载与编辑/imgs/image-20220514193704671.png)
+
+> 生命周期节点转换图如下：
+>
+> ![The proposed node life cycle state machine](1.ROS2地图加载与编辑/imgs/life_cycle_sm.png)
+
+
+
+## 4.地图编辑
+
+最后我们来说一下如何对地图进行编辑，地图编辑的方法有很多，你可以手动改图片，也可以通过opencv等图像处理库进行图像的去除噪点等操作。
+
+这里小鱼介绍的是手动添加一个地图用PS进行编辑 。
+
+打开网址：https://www.gaituya.com/ps/
+
+接着点击文件打开选择我们的.pgm文件，即可看到下图。
+
+![image-20220514221803090](1.ROS2地图加载与编辑/imgs/image-20220514221803090.png)
+
+接着小鱼将下面的正方形给补上。
+
+![edit_map](1.ROS2地图加载与编辑/imgs/edit_map.gif)
+
+保存后在文件选择导出为`png`格式即可，因为PS并不支持`pgm`格式的导出，所以我们选择`png`格式，幸运的是`png`格式的地图也是被map_server所支持的。
+
+把导出的图片放到map文件夹下，接着我们需要修改下yaml配置文件中图片的后缀。
+
+```
+image: fishbot_map.png
+mode: trinary
+resolution: 0.05
+origin: [-3.37, -2.88, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25
+```
+
+接着你可以重新运行下map_server（记得先关闭rviz2），然后看一下map是否发生了改变。
+
+## 5. 总结
+
+本节我们主要学习了地图的相关概念以及加载和编辑的方式，下一节我们就开始正式学习Nav2导航框架。
+
+
+
+参考文章：
+
+- https://zhuanlan.zhihu.com/p/21738718
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

docs/humble/chapt10/basic/1.图像常见格式及存储.md

@@ -0,0 +1,52 @@
+# 1.图像常见格式及存储
+
+在日常生活中，我们会用到各种各样的地图，比如交通轨道图、城市地图、世界地图。
+
+## 1.地图分类
+
+我们根据地图所表达信息的不同可以将地图分为三类：
+
+### 1.1 尺度地图（Metric Map）
+
+尺度地图用于表示尺寸距离，可以理解为把真实世界按比例缩小，尺度地图中每个点都可以使用一个经纬值进行表示。
+
+> 因为尺度地图是按照真实世界按比例缩小，所以要有比例尺来表示缩小比例。
+
+### 1.2 拓扑地图（Topological Map）
+
+拓扑地图用于表示点与点之间的连接信息，比如地铁轨道交通图。
+
+### 1.3 语义地图（Semantic Map）
+
+语义地图可以理解为在上面两个地图上增加了语义，比如尺度地图中某处是红绿灯、斑马线。拓扑地图中某个点是深圳北站。
+
+在机器人领域，尺度地图常用于定位于地图构建（Mapping）、定位（Localization）和同时定位与地图构建（Simultaneous Localization And Mapping，SLAM），拓扑地图常用于路径规划（Path Planning），而语义地图常用于人机交互（Human Robot Interaction）。
+
+
+
+## 2.地图的存储
+
+地图也是图，所以地图一般以图片的形式存储下来，常见的格式比如`png`就是可以存储ROS2中的二二维地图的。
+
+但是对于一些三维的点云图，二维的方式就不怎么好存储，就需要一些特殊格式了，因为我们后面不涉及三维空间建图，这里就不在展开了。
+
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划
+
+
+
+
+
+
+
+
+

docs/humble/chapt10/basic/2.栅格地图介绍.md

@@ -0,0 +1,40 @@
+# 2.占据栅格地图
+
+我们先了解下什么是栅格地图（Grid Map）。
+
+![栅格数据集图](2.栅格地图介绍/imgs/20140206174127486.gif)
+
+如上图将地图数据分割为一块块的栅格来表达地图信息，就是栅格地图。
+
+那什么是占据（Occupancy）呢？
+
+机器人通过激光雷达等传感器来感知深度信息，但我们知道，传感器都是有噪声的（在前面的机器人仿真建模过程中，我们为了更加真实给激光雷达还添加了高斯噪声），所以机器人前方的某个位置到底有没有物体（障碍物）是不确定的。
+
+我们可以采用概率来解决这一问题，认为确实有物体的栅格的占据率为100%，确定没有物体的栅格占据率为0%，不确定的栅格就用（确认占据概率/确认非占据概率）值表示占据率。
+
+
+由此我们知道**占据栅格地图就是一张写满占据率的格子组成的地图**。
+
+![image-20220506134542099](2.栅格地图介绍/imgs/image-20220506134542099.png)
+
+> 我们在做机器人的路径规划的时候，需要确定一个格子是有障碍物？没障碍物？还是未知呢？所以我们一般会设定两个阈值：
+>
+> - 占据阈值（occupied_thresh），比如0.65，则表示栅格占据率大于0.65的认为是有障碍物。
+> - 空闲阈值（free_thresh），比如0.25，则表示栅格占据率小于0.25的认为没有障碍物。
+>
+> 那在free_thresh和occupied_thresh之间的则认为是未知区域（未探索）。
+
+
+
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划
+

docs/humble/chapt10/get_started/1.SLAM前世今生.md

@@ -0,0 +1,89 @@
+# 10.3 SLAM技术概述
+
+小伙伴们好，上一节我们搭建好了Gazebo中的仿真环境，本节课我们一起来了解一下SLAM技术。
+
+第一节中我们知道，要解决机器人自主导航问题就需要有感知（建图和定位）参与，通过感知输出机器人当前环境的地图信息和位置。而本节要讲的SLAM就是解决地图和定位问题的。
+
+## 1. SLAM是什么
+
+SLAM是同步定位与地图构建(Simultaneous Localization And Mapping)的缩写。
+
+先通过一个视频直观了解一下
+
+<iframe height="400" width="600" src="//player.bilibili.com/player.html?aid=724624445&bvid=BV1tS4y1S7qE&cid=544765528&page=1" scrolling="no" border="0" frameborder="no" framespacing="0" allowfullscreen="false"> </iframe>
+
+视频中随着机器人的移动，机器人的传感器获取到了环境信息，然后完成对博物馆地图的构建。
+
+> 有小伙伴可能会问，只看到建图没看到定位呀。细心观察可以发现建图的过程中，其实一直都在计算机器人的位置。
+
+## 2. SLAM如何解决建图定位问题
+
+机器人通过自身传感器数据处理进行位置估计，同时通过不断移动完成对整个未知环境的地图构建。这就是SLAM解决的问题。
+
+那又是如何解决的呢？SLAM实现的方案很多，但是几个比较关键的技术如下：
+
+1. 传感器感知
+   通过各类传感器实现对环境的感知，比如通过激光雷达获取环境的深度信息。同时可以通过传感器融合来提高位置估计的精度，比如融合轮式里程计、IMU、雷达、深度相机数据等。
+
+2. 视觉/激光里程计
+   基本原理是通过前后数据之间对比得出机器人位置的变化。
+
+3. 回环检测
+   判断机器人是否到达之前到过的位置，可以解决位置估计误差问题，建图时可以纠正地图误差。
+
+
+经典视觉SLAM结构
+
+![经典视觉SLAM结构](../../../foxy/chapt10/10.3SLAM技术概述/imgs/image-20220421152216184.png)
+
+## 3.SLAM算法分类
+
+从算法的对数据的处理方式上看，目前常用的SLAM开源算法可以分为两类
+
+1.基于滤波，比如扩展卡尔曼滤波（EKF: Extended Kalman Filter）、粒子滤波(PF: Particle Filter)等。
+
+ROS中的gmapping、hector_slam算法都是基于滤波实现的。
+
+2.基于图优化，先通过传感器进行构图，然后对图进行优化。
+
+目前比较主流的是图优化的方法，Cartographer就是基于图优化实现的。图优化相对于滤波，不用实时的进行计算，效率更高，消耗的资源更少，所以在实际场景中使用的更多。
+
+## 4.SLAM开源库
+
+### 4.1. Cartographer
+
+github地址：https://github.com/cartographer-project/cartographer
+
+Cartographer是一个可跨多个平台和传感器配置以2D和3D形式提供实时同时定位和建图（SLAM）的系统。
+
+### 4.2. ORB_SLAM2(纯视觉)
+
+github地址：https://github.com/raulmur/ORB_SLAM2
+
+ORB-SLAM2是用于单目，双目和RGB-D相机的实时SLAM库，用于计算相机轨迹和稀疏3D重建
+
+### 4.3 VINS
+
+github地址：https://github.com/HKUST-Aerial-Robotics/VINS-Mono
+
+VINS-Mono是单目视觉惯性系统的实时SLAM框架。它使用基于优化的滑动窗口配方来提供高精度的视觉惯性测距。
+
+## 5.总结
+
+本节课我们简单介绍了下感知部分的技术担当SLAM，并对常用的开源库进行介绍，下一节我们就对其中的Cartograpger开源库进行介绍和安装。
+
+
+
+参考文章：
+
+- https://blog.csdn.net/heyijia0327/article/details/47686523
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

docs/humble/chapt10/get_started/2.Carto介绍及安装.md

@@ -0,0 +1,226 @@
+# 2. Cartographer介绍与安装
+
+## 1.Cartographer介绍
+
+
+![cartographer](2.Carto介绍及安装/imgs/cartographer.gif)
+
+Cartographer是Google开源的一个可跨多个平台和传感器配置以2D和3D形式提供实时同时定位和建图（SLAM）的系统。
+
+> github地址：https://github.com/cartographer-project/cartographer
+> 文档地址：https://google-cartographer.readthedocs.io/en/latest
+
+在小鱼呆过的机器人公司里，建图最终方案都是采用了Cartographer，甚至花费大量人力物力对Cartographer算法进行裁剪，这足以表明Cartographer算法的优越性。
+
+> Cartographer系统架构概述（简单看看即可，如果大家后面确定研究方向是SLAM可以深入学习）：
+>
+> 简单的可以看到左边的可选的输入有深度信息、里程计信息、IMU数据、固定Frame姿态。
+>
+> 
+>
+> ![系统概述](2.Carto介绍及安装/imgs/high_level_system_overview.png)
+
+
+## 2.Carttographer安装
+
+### 2.1 apt安装
+
+安装`carotgrapher`
+
+```shell
+sudo apt install ros-humble-cartographer
+```
+
+需要注意我们不是直接使用`cartographer`，而是通过`cartographer-ros`功能包进行相关操作，所以我们还需要安装下`cartographer-ros`
+
+```shell
+sudo apt install ros-humble-cartographer-ros
+```
+
+### 2.2 源码安装
+
+小鱼比较推荐源码安装的方式，毕竟是以学习为目的，我们后面要稍微看一下源码。
+
+将下面的源码克隆到fishbot_ws的src目录下：
+
+```shell
+git clone https://ghproxy.com/https://github.com/ros2/cartographer.git -b ros2
+git clone https://ghproxy.com/https://github.com/ros2/cartographer_ros.git -b ros2
+```
+
+#### 安装依赖
+
+这里我们使用rosdepc进行依赖的安装，rosdepc指令找不到可以先运行下面的一键安装命令，选择一键配置rosdep即可。
+
+```shell
+wget http://fishros.com/install -O fishros && . fishros
+```
+
+接着在fishbot_ws下运行下面这个命令进行依赖的安装。
+
+> rosdepc 是小鱼制作的国内版rosdep，是一个用于安装依赖的工具。该工具的安装可以采用[一键安装](https://fishros.org.cn/forum/topic/20)进行，选项编号为3。安装完成后运行一次rodepc update即可使用。
+
+```shell
+rosdepc install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
+```
+
+#### 编译
+
+> 这里有一个新的命令--packages-up-to，意思是其所有依赖后再编译该包
+
+```shell
+colcon build --packages-up-to cartographer_ros
+```
+
+### 2.3 测试是否安装成功
+
+如果是源码编译请先source下工作空间后再使用下面指令查看是否安装成功；
+
+```shell
+ros2 pkg list | grep cartographer
+```
+
+能看到下面的结果即可
+
+```shell
+cartographer_ros
+cartographer_ros_msgs
+```
+
+> 可能你会好奇为什么没有cartographer，因为cartographer包的编译类型原因造成的，不过没关系，cartographer_ros依赖于cartographer，所以有cartographer_ros一定有cartographer。
+
+## 3.Cartographer参数配置
+
+作为一个优秀的开源库，Cartographer提供了很多可以配置的参数，虽然灵活性提高了，但同时也提高了使用难度（需要对参数进行调节配置），所以有必要在正式使用前对参数进行基本的介绍。
+
+因为我们主要使用其进行2D的建图定位，所以我们只需要关注2D相关的参数。
+
+Cartographer参数是使用lua文件来描述的，不会lua也没关系，我们只是改改参数而已。
+
+> 小鱼提示：lua中的注释采用 -- 开头
+
+### 3.1 前端参数
+
+**文件：trajectory_builder_2d** 
+
+`src/cartographer/configuration_files/trajectory_builder_2d.lua`
+
+请你打开这个文件自行浏览，小鱼对其中我们可能会在初次建图配置的参数进行介绍。
+
+```lua
+  -- 是否使用IMU数据
+  use_imu_data = true, 
+  -- 深度数据最小范围
+  min_range = 0.,
+  -- 深度数据最大范围
+  max_range = 30.,
+  -- 传感器数据超出有效范围最大值时，按此值来处理
+  missing_data_ray_length = 5.,
+  -- 是否使用实时回环检测来进行前端的扫描匹配
+  use_online_correlative_scan_matching = true
+  -- 运动过滤，检测运动变化，避免机器人静止时插入数据
+  motion_filter.max_angle_radians
+```
+
+### 3.2 后端参数
+
+**文件：pose_graph.lua**-后端参数配置项
+
+路径`src/cartographer/configuration_files/pose_graph.lua`
+
+该文件主要和地图构建
+
+```lua
+--Fast csm的最低分数，高于此分数才进行优化。
+constraint_builder.min_score = 0.65
+--全局定位最小分数，低于此分数则认为目前全局定位不准确
+constraint_builder.global_localization_min_score = 0.7
+```
+
+### 3.3 Carotgrapher_ROS参数配置
+
+该部分参数主要是用于和ROS2进行通信和数据收发的配置，比如配置从哪个话题读取里程记数据，从哪个话题来获取深度信息（雷达）。
+
+**文件：backpack_2d.lua**
+
+路径：`src/cartographer_ros/cartographer_ros/configuration_files/backpack_2d.lua`
+
+```lua
+include "map_builder.lua"
+include "trajectory_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  -- 用来发布子地图的ROS坐标系ID，位姿的父坐标系，通常是map。
+  map_frame = "map",
+  -- SLAM算法跟随的坐标系ID
+  tracking_frame = "base_link",
+  -- 将发布map到published_frame之间的tf
+  published_frame = "base_link",
+  -- 位于“published_frame ”和“map_frame”之间，用来发布本地SLAM结果（非闭环），通常是“odom”
+  odom_frame = "odom",
+  -- 是否提供里程计
+  provide_odom_frame = true,
+  -- 只发布二维位姿态（不包含俯仰角）
+  publish_frame_projected_to_2d = false,
+  -- 是否使用里程计数据
+  use_odometry = false,
+  -- 是否使用GPS定位
+  use_nav_sat = false,
+  -- 是否使用路标
+  use_landmarks = false,
+  -- 订阅的laser scan topics的个数
+  num_laser_scans = 0,
+  -- 订阅多回波技术laser scan topics的个数
+  num_multi_echo_laser_scans = 1,
+  -- 分割雷达数据的个数
+  num_subdivisions_per_laser_scan = 10,
+  -- 订阅的点云topics的个数
+  num_point_clouds = 0,
+  -- 使用tf2查找变换的超时秒数
+  lookup_transform_timeout_sec = 0.2,
+  -- 发布submap的周期间隔
+  submap_publish_period_sec = 0.3,
+  -- 发布姿态的周期间隔
+  pose_publish_period_sec = 5e-3,
+  -- 轨迹发布周期间隔
+  trajectory_publish_period_sec = 30e-3,
+  -- 测距仪的采样率
+  rangefinder_sampling_ratio = 1.,
+  --里程记数据采样率
+  odometry_sampling_ratio = 1.,
+  -- 固定的frame位姿采样率
+  fixed_frame_pose_sampling_ratio = 1.,
+  -- IMU数据采样率
+  imu_sampling_ratio = 1.,
+  -- 路标采样率
+  landmarks_sampling_ratio = 1.,
+}
+```
+
+
+
+## 4.总结
+
+本节我们简单的介绍了Cartographer以及二进制和源码安装的方法，并对参数进行介绍。
+
+下一节我们就开始为fishbot配置cartographer，接着就可以使用fishbot进行建图了。
+
+
+
+参考文章：
+
+- https://google-cartographer.readthedocs.io/en/latest/configuration.html
+
+  
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

docs/humble/chapt10/get_started/3.配置FishBot进行建图.md

@@ -0,0 +1,336 @@
+# 3. 配置Fishbot进行建图
+
+上一节我们安装好了cartographer，这节课我们就开始配置cartographer进行建图。
+
+我们需要创建一个功能包，将参数文件和`Cartographer`启动文件放到一起然后启动。
+
+![cartographer](3.配置FishBot进行建图/imgs/cartographer.gif)
+
+## 1.创建fishbot_cartographer
+
+在src目录下，使用创建功能包指令，创建功能包
+
+```shell
+cd src
+ros2 pkg create fishbot_cartographer
+```
+
+接着创建配置文件夹、launch文件夹和rviz配置文件夹。
+
+```shell
+cd fishbot_cartographer
+mkdir config
+mkdir launch
+mkdir rviz
+```
+
+创建完成的功能包结构
+
+```
+.
+├── CMakeLists.txt
+├── config
+├── launch
+├── src
+├── package.xml
+└── rviz
+```
+
+## 2.添加cartographer配置文件
+
+在`fishbot/config`目录下创建`fishbot_2d.lua`文件。
+
+接着我们来写一下配置文件，相较于默认的配置文件，主要修改以下内容（见注释）
+
+```lua
+include "map_builder.lua"
+include "trajectory_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame = "map",
+  tracking_frame = "base_link",
+  -- base_link改为odom,发布map到odom之间的位姿态
+  published_frame = "odom",
+  odom_frame = "odom",
+  -- true改为false，不用提供里程计数据
+  provide_odom_frame = false,
+  -- false改为true，仅发布2D位资
+  publish_frame_projected_to_2d = true,
+  -- false改为true，使用里程计数据
+  use_odometry = true,
+  use_nav_sat = false,
+  use_landmarks = false,
+  -- 0改为1,使用一个雷达
+  num_laser_scans = 1,
+  -- 1改为0，不使用多波雷达
+  num_multi_echo_laser_scans = 0,
+  -- 10改为1，1/1=1等于不分割
+  num_subdivisions_per_laser_scan = 1,
+  num_point_clouds = 0,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
+  rangefinder_sampling_ratio = 1.,
+  odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
+  imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
+}
+
+
+-- false改为true，启动2D SLAM
+MAP_BUILDER.use_trajectory_builder_2d = true
+
+-- 0改成0.10,比机器人半径小的都忽略
+TRAJECTORY_BUILDER_2D.min_range = 0.10
+-- 30改成3.5,限制在雷达最大扫描范围内，越小一般越精确些
+TRAJECTORY_BUILDER_2D.max_range = 3.5
+-- 5改成3,传感器数据超出有效范围最大值
+TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.
+-- true改成false,不使用IMU数据，大家可以开启，然后对比下效果
+TRAJECTORY_BUILDER_2D.use_imu_data = false
+-- false改成true,使用实时回环检测来进行前端的扫描匹配
+TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 
+-- 1.0改成0.1,提高对运动的敏感度
+TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
+
+-- 0.55改成0.65,Fast csm的最低分数，高于此分数才进行优化。
+POSE_GRAPH.constraint_builder.min_score = 0.65
+--0.6改成0.7,全局定位最小分数，低于此分数则认为目前全局定位不准确
+POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
+
+-- 设置0可关闭全局SLAM
+-- POSE_GRAPH.optimize_every_n_nodes = 0
+
+return options
+```
+
+
+
+## 3.添加launch文件
+
+### 3.1 launch需要包含的节点
+
+要完成使用Cartographer进行建图，需要两个节点的参与，整个过程的计算流图如下：
+
+![image-20220503212253994](3.配置FishBot进行建图/imgs/image-20220503212253994.png)
+
+**/cartographer_node节点:**
+
+该节点从/scan和/odom话题接收数据进行计算，输出/submap_list数据.
+
+该节点需要接收一个参数配置文件（第二部分写的那个）参数。
+
+**/occupancy_grid_node节点：**
+
+该节点接收/submap_list子图列表，然后将其拼接成map并发布
+
+该节点需要配置地图分辨率和更新周期两个参数。
+
+### 3.2 编写launch文件
+
+在路径`src/fishbot_cartographer/launch/`下新建`cartographer.launch.py`文件，接着我们将上面两个节点加入到这个launch文件中。
+
+我们在第二部分写的配置文件就是给cartographer_node节点的，可以通过这个节点启动参数`configuration_directory`和`configuration_basename`进行传递。
+
+```
+import os
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # 定位到功能包的地址
+    pkg_share = FindPackageShare(package='fishbot_cartographer').find('fishbot_cartographer')
+    
+    #=====================运行节点需要的配置=======================================================================
+    # 是否使用仿真时间，我们用gazebo，这里设置成true
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    # 地图的分辨率
+    resolution = LaunchConfiguration('resolution', default='0.05')
+    # 地图的发布周期
+    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
+    # 配置文件夹路径
+    configuration_directory = LaunchConfiguration('configuration_directory',default= os.path.join(pkg_share, 'config') )
+    # 配置文件
+    configuration_basename = LaunchConfiguration('configuration_basename', default='fishbot_2d.lua')
+
+    
+    #=====================声明三个节点，cartographer/occupancy_grid_node/rviz_node=================================
+    cartographer_node = Node(
+        package='cartographer_ros',
+        executable='cartographer_node',
+        name='cartographer_node',
+        output='screen',
+        parameters=[{'use_sim_time': use_sim_time}],
+        arguments=['-configuration_directory', configuration_directory,
+                   '-configuration_basename', configuration_basename])
+
+    occupancy_grid_node = Node(
+        package='cartographer_ros',
+        executable='occupancy_grid_node',
+        name='occupancy_grid_node',
+        output='screen',
+        parameters=[{'use_sim_time': use_sim_time}],
+        arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec])
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        # arguments=['-d', rviz_config_dir],
+        parameters=[{'use_sim_time': use_sim_time}],
+        output='screen')
+
+    #===============================================定义启动文件========================================================
+    ld = LaunchDescription()
+    ld.add_action(cartographer_node)
+    ld.add_action(occupancy_grid_node)
+    ld.add_action(rviz_node)
+
+    return ld
+```
+
+## 4.添加安装指令
+
+做完上面的操作，我们还需要添加安装指令。
+
+打开CmakeLists.txt，添加下面一条指令，将三个目录安装到install目录。
+
+```
+install(
+  DIRECTORY config launch rviz
+  DESTINATION share/${PROJECT_NAME}
+)
+```
+
+
+
+## 5.开始建图
+
+### 5.1编译启动
+
+```shell
+colcon build --packages-select fishbot_cartographer 
+```
+
+启动建图前，需要先启动gazebo仿真环境，因为我们的建图程序依赖于Gazebo提供雷达和里程计等数据。
+
+```shell
+source install/setup.bash
+ros2 launch fishbot_description gazebo.launch.py
+```
+
+source,启动建图
+
+```shell
+source install/setup.bash
+ros2 launch fishbot_cartographer cartographer.launch.py 
+```
+
+### 5.2修改配置
+
+如果一切正常，你应该看到的是一个空空如也的RVIZ界面
+
+![image-20220503221911478](3.配置FishBot进行建图/imgs/image-20220503221911478.png)
+
+不用担心，此时地图其实已经有了，我们需要添加一下地图相关的插件即可。
+
+通过`Add`->`By Topic`添加组件。
+
+![image-20220503222230429](3.配置FishBot进行建图/imgs/image-20220503222230429.png)
+
+最后通过左边的插件你应该可以看到图和机器人了。
+
+![image-20220503222158266](3.配置FishBot进行建图/imgs/image-20220503222158266.png)
+
+
+
+### 5.3开始建图
+
+打开我们机器人遥控节点，降低速度，控制机器人走一圈，看看地图的变化。
+
+```
+ros2 run teleop_twist_keyboard teleop_twist_keyboard 
+```
+
+![cartographer](3.配置FishBot进行建图/imgs/cartographer.gif)
+
+### 6.保存地图
+
+走完一圈，没有黑影部分，我们就可以保存地图为一个本地文件了。我们需要使用一个叫做`nav2_map_server`的工具。
+
+### 6.1 安装nav2_map_server
+
+```
+sudo apt install ros-humble-nav2-map-server
+```
+
+### 6.2 保存地图
+
+```
+ros2 run nav2_map_server map_saver_cli --help
+```
+
+可以看到有下面的用法
+
+```shell
+Usage:
+  map_saver_cli [arguments] [--ros-args ROS remapping args]
+
+Arguments:
+  -h/--help
+  -t <map_topic>
+  -f <mapname>
+  --occ <threshold_occupied>
+  --free <threshold_free>
+  --fmt <image_format>
+  --mode trinary(default)/scale/raw
+
+NOTE: --ros-args should be passed at the end of command line
+```
+
+我们的地图话题为map，文件名字我们用fishbot_map,所以有下面这个这样写的命令行。
+
+```shell
+# 先将地图保存到src/fishbot_cartographer/map目录下
+cd src/fishbot_cartographer/ && mkdir map && cd map
+ros2 run nav2_map_server map_saver_cli -t map -f fishbot_map
+```
+
+接着我们就可以得到下面的两个文件
+
+```shell
+.
+├── fishbot_map.pgm
+└── fishbot_map.yaml
+
+0 directories, 2 files
+```
+
+这两个文件就是对当前地图保存下来的文件，其中.pgm是地图的数据文件，.yaml后缀的是地图的描述文件。
+
+下面的导航过程中我们将要使用到地图文件进行路径的搜索和规划。
+
+
+
+## 7.总结
+
+本节小鱼带你一起完成了Fishbot的Cartographer配置和建图，下一节小鱼会对地图是什么以及如何使用进行介绍，接着我们就可以配置Nav2进行Fishbot的导航了。
+
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

docs/humble/chapt11/get_started/1.Nav2导航框架介绍.md

@@ -0,0 +1,319 @@
+# 1.Nav2导航框架介绍与安装
+
+上一节我们对机器人导航过程中所用到的2D地图进行了介绍，本节我们就来正式的学习下Nav2导航框架，关于Nav2相关的更多资料，请访问[Nav2中文网](https://nav2.fishros.com/)。
+
+## 1.Nav2是什么
+
+Nav2项目继承并发扬ROS导航栈的精神。该项目力求以安全的方式让移动机器人从A点移动到B点。Nav2也可以应用于其他应用，包括机器人导航，如下动态点跟踪，在这个过程中需要完成动态路径规划、计算电机的速度、避免障碍、恢复行为。
+
+## 2.Nav2如何做到的
+
+Nav2使用行为树调用模块化服务器来完成一个动作。动作可以是计算路径、控制力、恢复或任何其他与导航相关的操作。这些都是通过ROS Action服务器与行为树 (BT) 通信的独立节点。下图可以让你对Nav2的架构有一个很好的初步了解。
+
+![Navigation2框架图](1.Nav2导航框架介绍/imgs/architectural_diagram-16525447663514.png)
+
+
+
+### 2.2 四个服务
+
+小鱼再对上面的架构图进一步的进行解释，可以简单分为一大三小四个服务。
+
+一大：
+
+1. BT Navigator Server 导航行为树服务，通过这个大的服务来进行下面三个小服务组织和调用。
+
+三小：
+
+1. Planner Server，规划服务器，其任务是计算完成一些目标函数的路径。根据所选的命名法和算法，该路径也可以称为路线。说白了就是在地图上找路。
+2. Controller Server，控制服务器，在ROS 1中也被称为局部规划器，是我们跟随全局计算路径或完成局部任务的方法。说白了就是根据找的路控制机器人走路。
+3. Recovery Server，恢复服务器，恢复器是容错系统的支柱。恢复器的目标是处理系统的未知状况或故障状况并自主处理这些状况。说白了就是机器人可能出现意外的时候想办法让其正常，比如走着走着掉坑如何爬出来。
+
+通过`规划路径`、`控制机器人沿着路径运动`、`遇到问题自主恢复`三者进行不断切换完成机器人的自主导航（在这个过程还需要很多节点和数据的辅助，详建下面的组建部分）。
+
+### 2.3 两大代价（成本）地图
+
+在机器人导航的时候，仅仅靠一张SLAM建立的原始地图是不够的，机器人在运动过程中可能会出现新的障碍物，也有可能发现原始地图中某一块的障碍物消失了，所以在机器人导航过程中维护的地图是一个动态的地图，根据更新频率方式和用途不同，可以分为下面两种。
+
+> 注意：不论是全局代价地图还是局部代价地图都是有很多个图层共同叠加而成的，你也可以自己定义图层。
+
+#### 2.3.1 全局代价地图 （Global Costmap）
+
+全局代价地图主要用于全局的路径规划器。从上面结构图中其在可以看到在Planner Server中。
+
+通常包含的图层有：
+
+- Static Map Layer：静态地图层，通常都是SLAM建立完成的静态地图。
+- Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
+- Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。
+
+#### 2.3.2 局部代价地图（Local Costmap）
+
+局部代价地图主要用于局部的路径规划器。从上面结构图中其在可以看到在Controller Server中。
+
+通常包含的图层有：
+
+- Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
+- Inflation Layer：膨胀层，在障碍地图层上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。
+
+## 3.Nav2导航相关概念
+
+该部分摘抄自Nav2中文网，中间删除了一些信息，全部文章请访问[原文页面](http://dev.nav2.fishros.com/doc/concepts/index.html)。
+
+### 3.1 动作服务器（Action Server）
+
+动作服务器在[第五章中](../chapt5/5.4ROS2_Action介绍.md)我们有简单介绍过，Nav2框架中大量的才用了这种通信方式，并将起分装成了行为树（关于行为树下面会介绍）的基础节点进行调用。
+
+通过动作服务器通信，来计算路径规划、控制机器人运动和恢复。每个动作服务器都有自己独特的 `nav2_msgs` 格式的 `.action` 类型，用于与服务器进行交互。 
+
+### 3.2 生命周期节点和绑定
+
+生命周期 (或被管理的，更正确的) 节点是ROS 2独有的。更多信息可以是 [在这里](https://design.ros2.org/articles/node_lifecycle.html) 。它们是包含状态机转换的用于加载和卸载ROS 2服务器的节点。这有助于确定ROS系统启动和关闭的状态是否正常。
+
+**生命周期节点框架在整个项目中被广泛使用，所有服务器都使用它。如果可能的话，所有ROS系统最好使用生命周期节点。** 
+
+上一节我们通过命令行来配置和激活map_server节点就是因为map_server节点采用了生命周期节点进行管理。
+
+### 3.3 行为树
+
+行为树 (BT) 在复杂的机器人任务中变得越来越普遍。它们是待完成任务的树形结构。行为树为定义多步或多状态应用程序创建了一个更具可扩展性和人类可理解性的框架。这与有限状态机 (FSM) 相反，后者可能有几十个状态和数百个状态过渡。
+
+>  一个例子就是踢足球机器人。将足球比赛的逻辑嵌入FSM将具有挑战性，且容易出错因为有许多可能的状态和规则。此外，像从左侧、右侧或中间射门这样的建模选择尤其不清楚。使用行为树则可以为许多行为创建和重用基本原语，像 "kick" "walk" "go to ball" 。更多信息可以在 [这本书](https://arxiv.org/abs/1709.00084) 找到。强烈建议阅读第1-3章，以更好地理解术语和工作流程。大约需要30分钟。 
+
+Nav2项目使用 [BehaviorTree CPP V3](https://www.behaviortree.dev/) 作为行为树库。在 `BT Navigator` 中，创建了可以构建为行为树的节点插件。将节点插件加载到BT中，并且在解析该行为树的XML文件时，将关联注册的名称。此时，我们可以通过该行为树进行导航。 
+
+> 使用此库的一个原因是它能够加载子树。这意味着可以将Nav2行为树可以进行套娃操作。举个例子是在足球比赛中，使用Nav2行为树作为 "go to ball" 节点，将足球检测作为更大任务的一部分。此外，为BT提供了一个 `NavigateToPoseAction` 插件，因此可以从客户端应用程序通过通常的动作接口调用Nav2软件导航栈。 
+
+### 3.4 导航服务器
+
+规划器和控制器是导航任务的核心。恢复器用于使机器人摆脱不良状态或尝试处理各种形式的问题，以使系统具有容错能力。在本节中，将分析有关它们的一般概念及其在Nav2项目中的用途。 
+
+#### 3.4.1 规划器，控制器和恢复服务器 
+
+该项目中的三个Action Server是规划器、恢复器和控制器服务器。这些Action Server用于托管一个地图算法插件，以完成各种任务。它们还托管由该算法插件使用的环境表达，以计算其输出。 
+
+**规划器（Planners）**
+
+规划器的任务是计算完成一些目标函数的路径。根据所选的命名法和算法，该路径也可以称为路线。两个典型示例是计算一个到达目标位姿的规划（例如从当前位置到达一个目标位姿）或者完全覆盖（例如覆盖所有空闲空间的规划）。规划器可以访问全局环境表达和缓存在其中的传感器数据。规划器可以被编写为具有以下功能的工具： 
+
+* 计算最短路径
+* 计算完整覆盖路径 
+* 沿稀疏或预定义路线计算路径 
+
+*Nav2中规划器的一般任务是计算从当前位置到达目标位姿的一个有效且可能是最佳的路径。*
+
+**控制器 （Controllers）**
+
+控制器，在ROS 1中也被称为局部规划器，是我们跟随全局计算路径或完成局部任务的方法。控制器有权访问局部环境表达，以尝试计算要跟随的基准路径的可行控制工作。许多控制器会将机器人向前投射到空间中，并在每次更新迭代时计算局部可行路径。控制器可以被编写为具有以下功能的工具： 
+
+* 跟随路径 
+* 使用里程计坐标系中的检测器与充电站（桩）对接
+* 登上电梯
+* 与某个工具的接口 
+
+在Nav2中，控制器的一般任务是计算一个有效的控制工作以跟随全局规划路径。然而，有多个控制器类和局部规划器类。Nav2项目的目标就是所有控制器算法都可以作为此服务器中的插件，以用于一般研究和产业任务中。 
+
+**恢复器 （Recovery）**
+
+恢复器是容错系统的支柱。恢复器的目标是处理系统的未知状况或故障状况并自主处理这些状况。例子包括感知系统中会导致环境表达充满假障碍物的故障。这样就会触发清除成本地图恢复以允许机器人移动。 
+
+另一个例子就是机器人由于动态障碍物或控制不佳而卡住。在允许的情况下，倒退或原地旋转会允许机器人从卡住的位置移动到可以成功进行导航的自由空间中。 
+
+最后，在完全故障的情况下，可以实施恢复以引起操作员的注意以寻求帮助。这可以通过电子邮件、短信、Slack、Matrix等来完成。 
+
+#### 3.4.2 航点跟随 
+
+航点跟随是导航系统的基本功能之一。它会告知系统如何使用导航程序到达多个目的地。 
+
+`nav2_waypoint_follower` 软件包含一个航路点跟踪程序，该程序具有特定任务执行程序的插件接口。如果需要让机器人前往给定位姿并完成像拍照、捡起盒子或等待用户输入之类的特定任务，这会非常有用。 
+
+> 关于机器人队管理器/调度器有两种思想流派：哑机器人+智能集中式调度器；智能机器人+哑集中式调度器。 
+>
+> 在第一种思想中， `nav2_waypoint_follower` 软件包足以创造一个产品级的机器人解决方案。由于自主系统/调度器在分配任务时会考虑机器人的姿势、电池电量、当前任务等因素，机器人上的应用程序只需要关心手头的任务，而不用关心完成系统要求任务的其他复杂因素。在这种情况下，应该将发送至航点跟随者的请求视为1个工作单元（例如，仓库中的1次拣货、1个安全巡逻循环、1个过道等）来执行任务，然后返回给调度器以进行下一个任务或者要求充电。在这种思想流派中，航点跟随应用程序只是导航软件堆栈之上和系统自主应用程序之下的一个步骤。 
+>
+> 在第二种思想中， `nav2_waypoint_follower` 软件包是一个不错的示例应用程序/概念证明，但确实需要机器人上的航点跟踪/自主系统来承担更多任务以制定健壮的解决方案。在这种情况下，应该使用 `nav2_behavior_tree` 软件包创建自定义应用程序级别的行为树，以使用导航来完成任务。这可以包含子树，例如在任务中检查充电状态以返回停泊坞，或者在更复杂的任务中处理1个以上的工作单元。很快，将会有一个 `nav2_bt_waypoint_follower` (名称有待调整)，它将允许用户更容易地创建此应用程序。在这个思想流派中，航点跟随应用程序与自主系统的联系更加紧密，或者在很多情况下，航点跟随应用程序就是自主系统本身。 
+>
+> 这两种思想流派并不能简单地说谁比谁更好，谁更好很大程度上取决于机器人正在完成何种任务、处于何种类型的环境中以及有何种可用的云资源。通常，对于既定的业务案例，这种区别非常明显。 
+
+### 3.5 状态估计（重要组件）
+
+> Nav2中，默认进行状态估计的组件是AMCL (Adaptive Monte Carlo Localization)自适应蒙特卡洛定位。nav2中对应的功能包是nav2_amcl。
+
+根据ROS社区标准，在导航项目中，需要提供两个主要的坐标转换。 `map` 到 `odom` 的坐标变换由定位系统 (定位，建图，SLAM)提供， `odom` 到 `base_link` 的坐标转换由里程计系统提供。 
+
+注解：无需在机器人上使用LIDAR即可使用导航系统。不需要使用基于激光雷达的防撞、定位或SLAM系统。但是，Nav2确实可以提供说明和支持使用激光雷达对这些系统进行尝试和真实实现。使用基于视觉或深度传感器的定位系统和使用其他传感器来避免碰撞可以同样成功。唯一的要求就是在选择具体实现方式时遵循REP-105标准。 
+
+> REP-105标准
+>
+> [REP 105](https://www.ros.org/reps/rep-0105.html) 定义了导航和更大的ROS生态系统所需的框架和约定。应始终遵循这些约定，以利用社区中丰富的定位、里程计和SLAM项目。
+>
+> 简而言之，REP-105至少必须为机器人构造一个包含``map`` -> `odom` -> `base_link` -> `[sensorframes]` 的完整 的TF树。TF2是 ROS 2中的时变坐标变换库，Nav2使用TF2来表达和获取时间同步的坐标变换。全球定位系统 (GPS、SLAM、动作捕捉Motion Capture) 的工作是至少要提供 `map-> odom` 的坐标转换。然后，里程计系统的作用是提供 `odom` -> `base_link` 的坐标转化。关于 `base_link` 的其余坐标转换应该是静态的，并应在 [URDF](http://wiki.ros.org/urdf) 中定义。
+
+### 3.6 全局定位: 定位与SLAM 
+
+全局定位系统 (GPS、SLAM、运动捕捉) 的工作是至少提供 `map` -> `odom` 的坐标转换。Nav2项目提供的 `amcl` 是一种基于粒子过滤器的自适应蒙特卡罗定位技术，用于静态地图的定位。Nav2还提供用于定位和生成静态映射的SLAM工具箱作为默认的SLAM算法。 
+
+这些方法还可能产生其他输出，包括位置话题、地图或其他元数据，但它们必须提供该转换才能有效。使用机器人定位可以将多种定位方法融合在一起，下面将详细讨论。 
+
+### 3.7 里程计（Odometry）
+
+里程计系统的作用是提供 `odom` -> `base_link` 的坐标转换。里程计可以来自许多数据源，包括激光雷达、车轮编码器、VIO和IMU。里程计的目标是提供基于机器人运动的平滑和连续的局部坐标系。全局定位系统会相对全局坐标的坐标变换进行更新，以解决里程计的漂移问题。 
+
+这个 [Robot Localization](https://github.com/cra-ros-pkg/robot_localization/) 通常用于这种融合。它将采用各种类型的 `N` 个传感器，并为TF和话题提供连续平滑的里程计。一个典型的移动机器人装置可能有来自车轮编码器或IMU的里程计以及融合在这个工作区内的视觉。 
+
+这样平滑输出就可用于精确运动的航行位置推算和在全局位置更新之间准确地更新机器人的位置。 
+
+### 3.8 环境表达（建模）
+
+环境表征是机器人感知环境的方式。它还充当各种算法和数据源的中心定位工具，以将它们的信息组合到一个空间中。这样，控制器、规划器和恢复器就可以使用该空间来安全有效地计算它们的任务。 
+
+#### 成本地图和图层 
+
+当前的环境表达是一个成本地图。成本地图是包含来自未知、空闲、占用或膨胀成本的单元格的规则2D单元格网格。然后搜索该成本地图以计算全局计划或采样以计算局部控制工作。 
+
+各种成本地图图层被实现为pluginlib插件，以将信息缓冲到成本地图中。这包括来自LIDAR、RADAR、声纳、深度传感器、图像传感器等的信息。最好在传感器数据输入到层本地图之前进行处理，但这取决于开发人员。 
+
+可以使用相机或深度传感器创建代价地图层来检测和跟踪场景中的障碍物，以避免碰撞。此外，可以创建层来基于一些规则或启发式算法来改变基础成本图。最后，它们可用于将实时数据缓冲到2D或3D世界中，以进行障碍物的二值化标记。 
+
+#### 成本地图过滤器 
+
+想象一下，您正在注释地图文件 (或任何图像文件)，以便根据注释地图中的位置执行特定操作。
+
+通过使用成本地图过滤器可以实现以下功能： 
+
+* 机器人永远不会进入的禁区/安全区。 
+* 限速区，机器人进入这些区域的最大速度将受到限制。
+* 机器人在工业环境和仓库中移动的首选通道。 
+
+#### 其他形式的环境表示
+
+存在各种其他形式的环境表征。包括: 
+
+* 梯度图，类似于成本地图，但梯度图会表达表面梯度以检查可穿越性 
+* 3D成本图，以3D形式表示空间，但这样就也需要3D规划和碰撞检测 
+* 网格图，类似于梯度图，但具有多个角度的表面网格 
+* Vector space ，接收传感器信息并使用机器学习算法来检测要跟踪的单个物品和位置，而不是对离散点进行缓冲区计算
+
+## 4.Nav2下载安装
+
+看完了第三节的是不是似懂非懂，接下来我们将通过实验进一步来对照着学习。
+
+Nav2的安装有两种方式，一种是通过二进制直接安装，另外一种是通过源码的方式进行安装，这里小鱼依然推荐源码的方式，毕竟我们还是要看一看Nav2的源码的。
+
+### 4.1 apt安装
+
+安装`nav2`
+
+```shell
+ sudo apt install ros-humble-nav2-*
+```
+
+### 4.2 源码安装
+
+将下面的源码克隆到fishbot_ws的src目录下：
+
+```shell
+git clone https://ghproxy.com/https://github.com/ros-planning/navigation2.git -b humble
+```
+
+#### 安装依赖
+
+这里我们使用rosdepc进行依赖的安装，rosdepc指令找不到可以先运行下面的一键安装命令，选择一键配置rosdep即可。
+
+```shell
+wget http://fishros.com/install -O fishros && . fishros
+```
+
+接着在fishbot_ws下运行下面这个命令进行依赖的安装。
+
+```shell
+rosdepc install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
+```
+
+#### 编译
+
+> 这里有一个新的命令--packages-up-to，意思是其所有依赖后再编译该包
+
+```shell
+colcon build --packages-up-to navigation2
+```
+
+### 4.3 测试是否安装成功
+
+如果是源码编译**请先source下工作空间后再使用下面指令查看**是否安装成功；
+
+```shell
+ros2 pkg list | grep navigation2
+```
+
+能看到下面的结果即可
+
+```shell
+navigation2
+```
+
+
+## 5.源码功能包拆解
+
+包含代码的功能包及其功能见下面列表
+
+```
+#==============控制器及其实现相关功能包======================#
+nav2_controller　｜　控制器
+nav2_dwb_controller | DWB控制器，Nav2控制器的一个实现
+nav2_regulated_pure_pursuit_controller | 纯追踪控制器，Nav2控制器的一个实现
+nav2_constrained_smoother
+
+#==============规划器及其实现相关功能包======================#
+nav2_planner | Nav2规划器
+nav2_navfn_planner　｜　navfn规划器，Nav2规划器的一个实现
+nav2_smac_planner | smac规划器，Nav2规划器的一个实现
+
+#=====================恢复器==============================#
+nav2_recoveries | Nav2恢复器
+
+#=====================行为树节点及其定义====================#
+nav2_bt_navigator |　导航行为树
+nav2_behavior_tree | 行为树节点插件定义
+
+#=====================地图和定位===========================#
+nav2_map_server　｜　地图服务器
+nav2_costmap_2d　｜　2D代价地图
+nav2_voxel_grid | 体素栅格
+nav2_amcl | 自适应蒙特卡洛定位。　　状态估计，输入地图、激光、里程计数据，输出机器人map和odom之间的位资关系。
+
+#=====================通用插件系统管理等====================#
+nav2_bringup | 启动入口
+nav2_common　｜　公共功能包
+nav2_msgs　｜　通信相关消息定义
+nav2_util | 常用工具
+nav2_lifecycle_manager |节点生命周期管理器　
+nav2_rviz_plugins | RVIZ插件
+
+#=====================核心定义============================#
+nav2_core　｜　Nav2核心包
+navigation2 | nav2导航汇总配置
+
+#=====================应用================================#
+nav2_waypoint_follower | 路点跟踪
+
+#=====================测试=================================#
+nav2_system_tests | 系统测试
+```
+
+上面的每个节点都有自己的参数可以配置，下一节我们就对个个节点的配置进行介绍并进行适配
+
+## 6.总结
+
+本节我们对Nav2进行了详细的介绍和安装，下一节我们就开始为FishBot配置Nav2，然后开始尝试让Nav2控制Fishbot自主的移动起来。
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划
+

docs/humble/chapt11/get_started/2.为FishBot配置Nav2.md

@@ -0,0 +1,186 @@
+# 2.为FishBot配置Nav2
+
+安装好了Nav2，我们开始针对我们的fishbot改变一些参数进行导航相关的参数配置。Nav2可配置的参数比起Cartographer更多，但是不要害怕，因为大多数参数我们可能不会改变。
+
+> 有关Nav2的更多参数介绍和详细的配置意义，可以参考**Nav2中文网配置指南**一节。
+
+本节主要准备两个文件给launch节点使用，第一个是地图文件，第二个是nav2参数文件。
+
+## 1.创建fishbot_navigation2
+
+### 1.1创建功能包
+
+和前面在Cartographer中一样，我们需要创建一个文件夹放置配置文件、launch文件、rviz配置和地图等。
+
+进入到src目录下，使用下面指令创建功能包：
+
+```shell
+ros2 pkg create fishbot_navigation2 --dependencies nav2_bringup
+```
+
+这里我们添加了一个依赖`nav2_bringup`，后面写launch文件要用到，这里提前添加一下依赖。
+
+创建完成后的目录结构：
+
+```
+.
+├── CMakeLists.txt
+├── include
+│   └── fishbot_navigation2
+├── package.xml
+└── src
+
+3 directories, 2 files
+```
+
+### 1.2 添加maps文件夹
+
+```
+cd src/fishbot_navigation2
+mkdir launch config maps param rviz
+```
+
+### 1.3 复制地图文件
+
+将上一节的地图文件复制到map文件夹下。
+
+复制完成后`fishbot_navigation2`的文件结构如下
+
+```
+.
+├── CMakeLists.txt
+├── config
+├── launch
+├── maps
+│   ├── fishbot_map.png
+│   ├── fishbot_map.pgm
+│   ├── fishbot_map.yaml
+├── package.xml
+├── param
+└── rviz
+
+5 directories, 5 files
+```
+
+## 2.添加Nav2配置文件
+
+### 2.1 创建参数文件
+
+我们需要配置的文件是Nav2的参数文件，同样的，贴心的Nav2已经为我们准备好了参数模板
+
+```
+src/navigation2/nav2_bringup/bringup/params/nav2_params.yaml
+```
+
+在`src/fishbot_navigation2/param/`目录下创建`fishbot_nav2.yaml`
+
+```shell
+cd src/fishbot_navigation2/param/
+touch fishbot_nav2.yaml
+```
+
+### 2.2 复制参数
+
+然后将`src/navigation2/nav2_bringup/bringup/params/nav2_params.yaml`的内容复制粘贴到`fishbot_nav2.yaml`文件中。
+
+> **参数文件中的参数是谁的？**
+>
+> 在[5.1.4章节保存参数](chapt5/5.1ROS2参数介绍?id=_4体验参数)中，我们曾用`ros2 param dump <node_name>`指令将某个节点的参数保存为一个`.yaml`格式的文件。`fishbot_nav2.yaml`文件就是保存Nav2相关节点参数的文件。
+
+## 3. 配置参数
+
+其实参数不配置也是可以将Nav2跑起来的，但是后期想要更改运行的效果就需要对参数进行修改，所以有必要大概了解下参数的配置项和含义查询方法和修改方法。
+
+### 3.1 参数列表
+
+| 编号 | 配置项            | 用途                                                  | 对应模块与参数详解                                           |
+| ---- | ----------------- | ----------------------------------------------------- | ------------------------------------------------------------ |
+| 1    | amcl              | 机器人定位                                            | [nav2_amcl](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-amcl.html) |
+| 2    | bt_navigator      | 导航行为树（用于加载行为树节点并根据xml配置进行调度） | [nav2_bt_navigator](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-bt-navigator.html),[nav2_behavior_tree](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-bt-xml.html) |
+| 3    | controller_server | 控制器服务器                                          | [nav2_controller](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-controller-server.html),[nav2_dwb_controller](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-dwb-controller.html),[nav2_regulated_pure_pursuit_controller](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-regulated-pp.html) |
+| 4    | planner_server    | 规划服务器                                            | [nav2_planner](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-planner-server.html),[nav2_navfn_planner](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-navfn.html),[smac_planner](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-smac-planner.html) |
+| 5    | recoveries_server | 恢复服务器                                            | [nav2_recoveries](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-recovery-server.html) |
+| 6    | local_costmap     | 局部代价地图                                          | [nav2_costmap_2d](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-costmaps.html),[static_layer](http://dev.nav2.fishros.com/doc/configuration/packages/costmap-plugins/static.html),[inflation_layer](http://dev.nav2.fishros.com/doc/configuration/packages/costmap-plugins/inflation.html) |
+| 7    | global_costmap    | 全局代价地图                                          | [nav2_costmap_2d](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-costmaps.html),[nav2_map_server](http://dev.nav2.fishros.com/doc/configuration/packages/configuring-map-server.html) |
+
+
+有关更多的Nav2所有参数的详细介绍，可以访问Nav2中文网中的配置指南章节，非常的详细，无比的具体。
+
+![](2.为FishBot配置Nav2/imgs/image-20220517203105467.png)
+
+### 3.2 配置机器人半径和碰撞半径
+
+在全局代价地图和局部代价地图配置用，默认的机器人半径是0.22，而我们fishbot的半径是0.12，所以需要修改机器人的半径为0.12。
+
+```yaml
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      robot_radius: 0.12
+      
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      robot_radius: 0.12
+```
+
+为了防止机器人发生碰撞，一般我们会给代价地图添加一个碰撞层（inflation_layer），在`local_costmap`和`global_costmap`配置中，你可以看到下面关于代价地图相关的配置：
+
+```yaml
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+```
+
+打开参数配置中的[inflation_layer](http://dev.nav2.fishros.com/doc/configuration/packages/costmap-plugins/inflation.html)，我们来看看其配置项和含义。
+
+此处截图
+
+可以看到`inflation_radius`默认0.55对fishbot来说可能有些大了，我们改小些。
+
+```
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.35
+```
+
+**以上就是以代价地图碰撞半径为例的配置方法，nav2可以配置的参数非常多，假如你在导航过程中遇到问题，根据问题的表现推断下是哪个模块中造成的，接着修改其对应参数，大概率就可以解决问题，解决不了的可以看源码详细分析。**
+
+### 3.3 配置frame_id和话题
+
+这里也不用配置，因为我们的fishbot话题名称和tf名称都是遵循着默认的话题的。
+
+如果你的机器人不是，或者你改变了话题，这里就需要重新配置。
+
+- 默认全局的坐标系：map
+- 默认里程计坐标系：odom
+- 默认雷达话题：scan
+- 默认机器人基坐标系：base_link
+- 默认地图话题：map
+
+## 4. 总结
+
+本节我们简单的了解了Nav2各个模块参数的配置方法和参数的介绍，下一节就开始编写文件正式建图。
+
+
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

docs/humble/chapt11/get_started/3.使用FishBot进行自主导航.md

@@ -0,0 +1,208 @@
+# 3.使用FishBot进行自主导航
+
+经过前面三节的铺垫，我们只需要再编写一个launch文件启动nav2就可以让fishbot自己动起来了。
+
+## 1.编写launch文件
+
+我们将地图、配置文件传递给nav2为我们提供好的launch文件中即可。
+
+> 再一个launch文件中包裹另一个功能包中的luanch文件采用的是`IncludeLaunchDescription`和`PythonLaunchDescriptionSource`
+
+```python
+'''
+作者: 小鱼
+公众号: 鱼香ROS
+QQ交流群: 2642868461
+描述: Nav2 launch启动文件
+'''
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    #=============================1.定位到包的地址=============================================================
+    fishbot_navigation2_dir = get_package_share_directory('fishbot_navigation2')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    
+    
+    #=============================2.声明参数，获取配置文件路径===================================================
+    # use_sim_time 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true') 
+    map_yaml_path = LaunchConfiguration('map',default=os.path.join(fishbot_navigation2_dir,'maps','fishbot_map.yaml'))
+    nav2_param_path = LaunchConfiguration('params_file',default=os.path.join(fishbot_navigation2_dir,'param','fishbot_nav2.yaml'))
+    rviz_config_dir = os.path.join(nav2_bringup_dir,'rviz','nav2_default_view.rviz')
+
+    #=============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
+    nav2_bringup_launch = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([nav2_bringup_dir,'/launch','/bringup_launch.py']),
+            launch_arguments={
+                'map': map_yaml_path,
+                'use_sim_time': use_sim_time,
+                'params_file': nav2_param_path}.items(),
+        )
+    rviz_node =  Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_config_dir],
+            parameters=[{'use_sim_time': use_sim_time}],
+            output='screen')
+    
+    return LaunchDescription([nav2_bringup_launch,rviz_node])
+```
+
+## 2.安装并添加依赖
+
+### 2.1 修改CMakeLists.txt
+
+添加install指令，将文件拷贝到install目录
+
+```cmake
+cmake_minimum_required(VERSION 3.5)
+project(fishbot_navigation2)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+install(
+  DIRECTORY launch param maps
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
+```
+
+### 2.2 添加依赖
+
+主要是添加这行` <exec_depend>nav2_bringup</exec_depend>`
+
+```xml
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fishbot_navigation2</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="sangxin2014@sina.com">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <exec_depend>nav2_bringup</exec_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
+```
+
+## 3.构建运行
+
+### 3.1 构建
+
+```shell
+colcon build --packages-up-to  fishbot_navigation2
+```
+
+### 3.2 运行
+
+#### 3.2.1 运行仿真
+
+```
+source install/setup.bash
+ros2 launch fishbot_description gazebo.launch.py
+```
+
+#### 3.2.2 运行Nav2
+
+```
+source install/setup.bash
+ros2 launch fishbot_navigation2 navigation2.launch.py
+```
+
+## 4.初始化位置
+
+启动后正常你应该在RVIZ2和终端看到一个错误，这是因为没有给定初始化位置（告诉机器人它在地图的大概位置）导致的。
+
+![image-20220519231957742](3.使用FishBot进行自主导航/imgs/image-20220519231957742.png)
+
+
+
+```shell
+[planner_server-5] [INFO] [1652973621.731976741] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
+[rviz2-10] [INFO] [1652973621.760971376] [rviz2]: Message Filter dropping message: frame 'odom' at time 0.000 for reason 'Unknown'
+[rviz2-10] [INFO] [1652973621.856298950] [rviz2]: Message Filter dropping message: frame 'laser_link' at time 4392.881 for reason 'Unknown'
+[rviz2-10] [INFO] [1652973621.951345246] [rviz2]: Message Filter dropping message: frame 'laser_link' at time 4392.981 for reason 'Unknown'
+[rviz2-10] [INFO] [1652973621.951468235] [rviz2]: Message Filter dropping message: frame 'odom' at time 0.000 for reason 'Unknown'
+[rviz2-10] [INFO] [1652973622.047860791] [rviz2]: Message Filter dropping message: frame 'laser_link' at time 4393.081 for reason 'Unknown'
+```
+
+通过RVIZ2的工具栏上的 `2D Pose Estimate` 可以给迷茫的fishbot指明“机生方向”。
+
+![image-20220519232134366](3.使用FishBot进行自主导航/imgs/image-20220519232134366.png)
+
+点击 `2D Pose Estimate` ，进行姿态初始化（选中机器人在Gazebo位置差不多的点，左键点击不要松开，移动鼠标给定方向），初始化完后，左边的Global Status 就正常了。
+
+![2d_estimate](3.使用FishBot进行自主导航/imgs/2d_estimate.gif)
+
+## 5.单点导航
+
+点击RVIZ2工具栏上的 就可以给fishbot安排一个目标点了，点击按钮，到地图上任意一点击鼠标左键，注意不要松开，移动鼠标给定一个方向。
+
+![navigation_fishbot2](3.使用FishBot进行自主导航/imgs/navigation_fishbot2.gif)
+
+## 6.多点（路点）导航
+
+观察左下角，有一个Nav2的Rviz2小插件，可以进行启动停止和导航模式的切换，点击 切换到路点模式。
+
+接着你可以使用工具栏的  按钮，给FishBot指定多个要移动的点，接着点击左下角的启动，就可以看到FishBot依次到达这些目标点。
+
+![fishbot_multi_point](3.使用FishBot进行自主导航/imgs/fishbot_multi_point.gif)
+
+
+
+## 7.查看机器人当前在地图中的位置
+
+在机器人导航过程中我们如何实时查看机器人在地图中的位置呢？我们可以通过tf进行查看。
+
+打开终端，输入指令：
+
+```shell
+ros2 run tf2_ros  tf2_echo map base_link
+```
+
+接着就可以看到下面的信息，旋转和变换位资数据了
+
+```shell
+[INFO] [1653215686.225862749] [tf2_echo]: Waiting for transform map ->  base_link: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
+At time 4873.528000000
+- Translation: [-0.009, -0.016, 0.076]
+- Rotation: in Quaternion [0.000, -0.000, -0.000, 1.000]
+At time 4874.514000000
+- Translation: [-0.009, -0.016, 0.076]
+- Rotation: in Quaternion [0.000, -0.000, -0.000, 1.000]
+```
+
+## 8.总结
+
+本节我们终于将FishBot的自主导航给跑了起来，不过我们都是通过RVIZ2的工具给机器人指定的目标点，除了工具栏的工具，我们还可以使用代码的方式发送目标点给FishBot，下一节小鱼就会带你一起学习Nav2的API（应用程序接口）。
+
+课后作业：
+
+1. 使用rqt_tf_tree观察tf树的结构
+2. 阅读下Nav2中文网配置指南章节内容（顺便挑几个代校准的段落进行校准）
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

docs/humble/chapt11/get_started/4.使用Nav2导航API进行导航.md

@@ -0,0 +1,102 @@
+# 10.10通过Nav2API进行导航
+
+Nav2的API其实是Nav2提供的一个Python库，通过该库你可以事先调用你的机器人进行简单的控制（比如导航到点）。
+
+很遗憾的是，该功能包官方并没有发布foxy版本的，再下一个galactic版本才开始正式发布。
+
+从2022年5月23号开始，教程将开始向humble版本迁移，该部分内容将在humble版本发布。
+
+## 1.导入nav2_simple_commander
+
+```
+from nav2_simple_commander.robot_navigator import BasicNavigator
+import rclpy
+from copy import deepcopy
+```
+
+初始化BasicNavigator
+
+```
+rclpy.init()
+nav = BasicNavigator()
+navigator.waitUntilNav2Active()
+```
+
+## 2.初始化位置
+
+```
+# ======================初始化位置，代替rviz2的2D Pose Estimate===============================
+initial_pose = PoseStamped()
+initial_pose.header.frame_id = 'map'
+initial_pose.header.stamp = navigator.get_clock().now().to_msg()
+initial_pose.pose.position.x = 0.0
+initial_pose.pose.position.y = 0.0
+initial_pose.pose.orientation.w = 1.0
+navigator.setInitialPose(initial_pose)
+```
+
+## 3.导航到点
+
+```
+#========================导航到目标点1===========================================
+goal_pose1 = deepcopy(initial_pose)
+goal_pose1.pose.position.x = 1.5
+nav.goToPose(goal_pose1)
+while not nav.isNavComplete():
+  feedback = nav.getFeedback()
+  #检查是否超时，超时则停止导航到点   
+  if feedback.navigation_duration > 600:
+    nav.cancelNav()
+
+
+#================================导航到目标点2==================================
+goal_pose2 = deepcopy(initial_pose)
+goal_pose2.pose.position.x = -1.5
+
+nav.goToPose(goal_pose2)
+while not nav.isNavComplete():
+  feedback = nav.getFeedback()
+  #检查是否超时，超时则停止导航到点   
+  if feedback.navigation_duration > 600:
+    nav.cancelNav()
+
+#===============================查看返回结果=====================================
+result = nav.getResult()
+if result == NavigationResult.SUCCEEDED:
+    print('Goal succeeded!')
+elif result == NavigationResult.CANCELED:
+    print('Goal was canceled!')
+elif result == NavigationResult.FAILED:
+    print('Goal failed!')
+```
+
+
+
+## 4.总结
+
+上面是对nav2_simple_commander的简单介绍，介于本教程目前主要维护foxy版本问题，暂时该部分无法使用，不过你可以使用galactic版本进行实践。
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划