一位刚刚会用ROS的小白阅读的第一个工程项目。

说明：

该附件的编写方法为set(CMAKE_BUILD_TYPE Debug)后通过vscode逐行调试。主机运行两个ros-noetic的docker容器，用一个容器调试far-planner另一个容器用于练习从far-planner源码中学到的知识，并在此记录了自己的收货与感受。

虽然rqt上有大量的节点，其大都都与仿真环境和局部规划器有关。使用ROS插件调试far-planner会发现call_stack中只有/graph_decoder、/far_rviz、/far_planner三个节点。其余节点来自autonomous_exploration_development_environment为局部规划和环境仿真与本论文无关。/far_rviz用于可视化与核心逻辑无关，所以本附录主要介绍/graph_decoder和/far_planner两个节点。

刚开始写的时候对C++的掌握是C with class，读到后面逐渐掌握了cmake、STL、智能指针、Eigen库、PCL库等，期间学到的C++知识点也部分记录在了这里。

阅读源码时参考了CSDN上far-planner源码解析系列文章（https://blog.csdn.net/weixin_39977764/article/details/123572389），针对其中部分内容与博客作者进行了交流探讨。CSDN上的系列文章主要按照文件架构对进行解析，逐文件介绍各函数的功能。优点是内容详。本篇文章按照代码运行顺序，介绍各回调函数、核心逻辑模块的主要功能并按照调用栈介绍各被调用子函数逻辑。同时穿插介绍了涉及到的C++语法、库函数、数学相关知识。

graphDecoder节点

input：

far_planner节点的/robot_vgraph

far_rviz的/save_file_dir 和 /read_file_dir

output：

/decoded_vgraph发送给far_planner节点

void GraphDecoder::Init()

指明发布、订阅的话题

订阅两个话题，保存和读取地图。

服务通信。服务通信要求用于偶然的、对时时性有要求的数据传输场景，说明其他节点会通过服务通信向该节点索要最新地图数据。

graphOut是一个vector容器，调用clear将其清空。

void GraphDecoder::Loop()

从主函数可以看出: 主要的逻辑应该放在话题通信的回调函数和服务通信上了.

void GraphDecoder::GraphCallBack(const visibility_graph_msg::GraphConstPtr& msg)

const visibility_graph_msg::Graph shared_graph = *msg;

订阅到的话题信息，命名为shared_graph。里面包含nodes

this->ResetGraph(received_graph_);

resetgraph的定义如下：

研究一下graphOut的数据结构：

Na指针Na栈, 那节点指针是什么呢? NodePtr的类型：

现在需要搞清楚什么是shared_ptr和NavNode。NavNode是自定义的一种结构体。

首先研究了一下第一行的语法

如果结构体中有 "NavNode() = default;" 和其他成员变量，那么这行代码告诉编译器生成一个默认构造函数。默认构造函数将对结构体中的所有成员变量进行默认初始化。对于std::size_t 类型的成员变量 "id", 它会被初始化为0。默认构造函数的目的是提供一种方法来创建类型的对象，并且不用显式地指定初始值。

一个点的例子如下图所示：根据字面意思可以猜出id、connect_nodes、position的含义，其他元素的具体含义还需要通过后续代码推断。

全文读后补充：FreeType表示凹凸性、surface_dirs表示多边形到前向和后向的方向，可以用于判断凹凸性。那几个is后面会有解释。

接下来要搞懂shared_ptr是什么东西。http://c.biancheng.net/view/7898.html。多个 shared_ptr 智能指针可以共同使用同一块堆内存。并且，由于该类型智能指针在实现上采用的是引用计数机制，即便有一个 shared_ptr 指针放弃了堆内存的“使用权”（引用计数减 1），也不会影响其他指向同一堆内存的 shared_ptr 指针（只有引用计数为 0 时，堆内存才会被自动释放）。使用例程：

至此，这行代码的含义就清晰了，GraphDecoder类中维护着一个名为 received_graph_ 的变量，该变量是一个vector，vector的元素是NavNode的智能指针sharedptr。每当回调函数收到一个新图时都会将旧图清空。

NavNodePtr temp_node_ptr = NULL;

接下来这几行应该是要初始化并处理某种数据，具体要干什么要读后面的代码，用到接收到的msg，有如下元素：

robot_id_ = msg->robot_id;

std::unordered_map<std::size_t, std::size_t> nodeIdx_idx_map;

map和unordered_map都是c++中可以充当字典（key-value）来用的数据类型，但是其基本实现是不一样的。unordered_map和map类似，都是存储的key-value的值，可以通过key快速索引到value。不同的是unordered_map不会根据key的大小进行排序，存储时是根据key的hash值判断元素是否相同，即unordered_map内部元素是无序的。unordered_map的底层是一个防冗余的哈希表(https://blog.csdn.net/jingyi130705008/article/details/82633778#:~:text=unordered_map%E4%BB%8B%E7%BB%8D%EF%BC%9A%20%E6%97%A0%E5%BA%8F%E6%98%A0%E5%B0%84,%E5%80%BC%E7%9A%84%E7%B1%BB%E5%9E%8B%E5%8F%AF%E8%83%BD%E4%B8%8D%E5%90%8C%E3%80%82)

全文读后补充：之后学习了map、set系列的六个容器，很常用不过没在此处记录。

for (std::size_t i=0; i<shared_graph.nodes.size(); i++) {

const auto node = shared_graph.nodes[i];

CreateNavNode(node, temp_node_ptr);

函数的两个输入分别是刚接收到的话题消息中的一个元素和刚刚初始化的一个指针。接下来看一下这个函数的操作。

node_ptr = std::make_shared<NavNode>();

make_shared函数的主要功能是在动态内存中分配一个对象并初始化它，返回指向此对象的shared_ptr

node_ptr->position = Point3D(msg.position.x, msg.position.y, msg.position.z);

node_ptr->id = msg.id;

node_ptr->free_direct = static_cast<NodeFreeDirect>(msg.FreeType);

NodeFreeDirect是一个枚举类型。static_cast是c++中的强制类型转换。用例如下：

关于这个枚举类型，UNKNOWN应该表示该类型未知，CONVEX和CONCAVE应该是描述多边形的形状，PILLAR类型，我找到一个有关的调用如下所示，可以推测PILLAR 表示该点和多边形的构建无关（不在轮廓线上）。

全文读后补充：PILLAR是指周长小的多边形或线段，这个命名很奇怪

node_ptr->free_direct = (is_odom || is_navpoint) ? NodeFreeDirect::PILLAR : NodeFreeDirect::UNKNOW;

对node_ptr的surf_dirs进行赋值，应该是方向，需要两个点。猜测是表示边的方向，根据论文后续要计算inner angle。读完全部代码后应该会再验证猜想。

接着对这四个属性进行赋值

这四个属性的说明在UpdateNavGraph函数中，接下来根据这个函数对这四个属性进行研究。

代码对node是否为iscovered进行判断是通过IsNodeFullyCovered函数进行的

node_ptr->connect_idxs.clear(), node_ptr->poly_idxs.clear(), node_ptr->contour_idxs.clear(), node_ptr->traj_idxs.clear();

对节点的临接点进行更新，更新之前将之前的删掉。

接下来四个循环都是把msg中的值赋值给节点指针，没什么好说的。

node_ptr->connect_nodes.clear(), node_ptr->poly_connects.clear(), node_ptr->contour_connects.clear(), node_ptr->traj_connects.clear();

赋完idxs后又将connects清空。

CreateNavNode和复制差不多，将原始类型转换为NavNode类型。用一张图总结一下这个函数的功能：CreateNavNode(node, temp_node_ptr);

这些idx的作用如下图所示，运算中获得节点的id，可以根据节点的id获得节点在nodes数组中的位置进而找到该节点的其他属性：

不过connects被清空了，清空是因为点的类型变了，从原始类型变成了nav类型。接下来需要将connects也变为nav类型，实现的方法就是把所有点的connects清空，其他部分变成nav后再重新添加connects。对应的是如下操作：

if (AddNodePtrToGraph(temp_node_ptr, received_graph_))

将NavNode添加到要输出的graph上。

读到这里对这个函数所做的事已经很清楚了，它对收到的shared_graph = *msg进行了一种加工。

最初的图由点组成，该函数首先通过CreateNavNode将原始类型的点转换为Nav类型的点：

接着制作了一张nodeIdx_idx_map，完成点id与物理空间排序的对应，这样就可以随机访问点。

接着根据map制作了connects，一张图就构造好了。

使用rviz进行可视化

MarkerArray graph_marker_array;

最后这个函数对处理后的图进行了可视化。

可视化前两行用到了Maker和MakerArray类型，先对这两种数据结构进行研究。

根据这两个文档对Maker进行学习，得知其功能为在rviz中显示想要的图形。随后实现了两个demo

https://blog.csdn.net/m0_45388819/article/details/113696345

https://www.guyuehome.com/14764

在rviz中显示一个球:

接下来研究下far-planner代码中使用的SPHERE_LIST等结构: list系列类似于vector容器，向其中push_backPoint类型即可，操作起来很方便。根据所学知识尝试实现一个demo:

根据上述知识，365-391行代码非常基础，此处不做讲解。

for (const auto& cnode : node_ptr->connect_nodes)

For (... : ...) c++循环语法，本系统中已经出现过多次，第二个...表示一个类似列表的数据结构，将其中的元素顺序赋给第一个元素。

这部分的意思是对输入点，遍历所有与之相连接的点。

全文读后补充：迭代器才能用，STL的语法，当时还是纯小白。

p1 = ToGeoMsgP(node_ptr->position);

ToGeoMsgP是作者自定义的赋值操作。

inline关键字第一次见是在vivadoHLS文档中, 内联函数, FPGA中该语法可以提高性能但是会增加资源占用，此处应该是类似的功能。

edge_marker.points.push_back(p1);edge_marker.points.push_back(p2);

上面的一行代码，一条线 get！下面循环遍历node_ptr->connect_nodes表示把所有和p1相连的点都连接上。

接下来一共操作了：

为了看懂这些点的含义，需要和rviz进行对照，在初始化函数中定义了这个pub的话题：

刚才地11步分析的代码是covered_node_marker，其rviz显示如下所示。可以看出其内容就是遍历graph上的每个点，连接与之connect的点。

poly_connects表示多边形。

通过类似的方式即可获得其他内容的含义，一个比较神奇的点是trajectory_vertex都被加到了global_vertex中（global_vertex不光有角点）。即下图的黄绿色点都兼是第一个图中的白色点。:

NodePtrStack received_graph_是一个类的属性（相当于全局变量）会在下个函数用到。不过graph_marker_array_定以后从来没被用到。

GraphDecoder::SaveGraphCallBack(const std_msgs::StringConstPtr& msg)

订阅/save_file_dir，当订阅到信息后，执行它的回调函数SaveGraphCallBack。

先进行异常检测：

接下来遍历每一个点，将相关信息输出到文件中。期间把自己定义的枚举类型强制转换为int类型输出。

画黄的部分是lambda表达式，截至目前已经出现三次，接下来学习一下相关语法。参考资料为chatgpt和王建伟《c++语言从入门到进阶》。

lambda表达式

全文读后补充：这个好方便，学了之后在自己写的代码里也经常用。

用法简介

编译器并不是总能推断出返回值类型, 会报错。没有参数的时候参数列表可以省略甚至（）也可以省略，捕获列表和函数体不能省略lambda调用方法和普通函数相同，都是函数调用运算符，lambda可以不返回任何类型，如下格式合法：

捕获列表

[&]表示捕获函数内所有变量并作为引用在匿名函数中使用。举例：下面两个函数的输出都是5

[=]表示捕获作用域中所有变量，并按值在函数中使用（只读）。

如何调用该回调函数

打上断点后等待良久该函数也没有运行，经过主动寻找，在src/teleop_rviz_plugin/src/teleop_panel.cpp中有相关代码：

对该cpp文件大致越大发现是在rviz上做GUI：Rviz_plugin是ROS中一个用于可视化的插件。它可以用来在Rviz中添加新的功能，比如显示自定义的3D模型、添加新的工具栏、创建自定义的可视化工具等。Rviz_plugin使用Ogre3D来渲染3D图形，并且可以与ROS系统中的数据交互。

点击save即可进入该函数：

执行逻辑解析

msg->data是在gui界面选择的路径，第三行又做了一次异常检查，可见该代码编者工程经验丰富。接下来两行创建文件用于存储数据。之后创建一个匿名函数：

接下来遍历类属性received_graph_（NodePtrStack类型）。逐点进行文件的写入操作。

void GraphDecoder::ReadGraphCallBack(const std_msgs::StringConstPtr& msg)

打上断电后在rviz中点击read进入。

初始化相关变量：

重载了一个函数，可以把读取到的一行解析为一个NavNode：

之后循环调用这个函数，将解析出来的值赋给temp_node_ptr，再通过和GraphCallBack相同的方法将其拼成一张图。并传递给this->VisualizeGraph让其在rviz中画出。接下来将图转换为可以发布的消息

函数的前三行很好懂，第四行是将图转换为GraphCallBack输入参数的类型，发布出去。

节点阅读总结

bool GraphDecoder::RequestGraphService(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)这是该节点的最后一个函数了，虽然不清楚这个系统如何调用这个节点，但该接口有极大概率是该节点的主逻辑。

函数的输入：std_srvs::Trigger::Request 是ROS系统中 std_srvs 包中的一个消息类型。请求消息 std_srvs::Trigger::Request 是一个空消息。当请求服务时，客户端只需要发送一个空请求，服务端收到请求后就会执行相应的操作。

函数的输出：std_srvs::Trigger::Response 是ROS系统中 std_srvs 包中的一个消息类型。std_srvs::Trigger::Response 消息包含了一些字段，用来表示服务执行的结果，这些字段包括:success (bool): 表示服务执行是否成功；message (string): 表示服务执行过程中的附加信息。当服务端执行完请求后，会将结果填充到 std_srvs::Trigger::Response 消息中并返回给客户端。

我将通过这个函数总结前文阅读成果。函数中相关（打杂的）代码有：

核心逻辑是：

visibility_graph_msg::Graph graph_msg; 初始化一个graph_msg

数据类型详解：visibility_graph_msg::Graph

visibility_graph_msg::Graph 消息类型包含了主要包含Nodes字段和图描述字段:

nodes (visibility_graph_msgs/Node[]): 一个数组，包含了所有节点的信息，其中每个节点包含了坐标、编号、是否为边界点、是否为导航点、是否为边界点等信息。

节点信息包括：

坐标xyz

编号id

是否为边界点is_boundary：is_boundary表示该节点是否位于空间中的边界。如果该节点位于空间边界上，则该变量为true，否则为false。举个例子，在一个地图上，边界就是地图的边缘，如果有一个点在地图的边缘，这个点的is_boundary就为true。

是否为边缘点is_frontier：is_frontier表示该节点是否位于空间中的边缘，也就是该点附近有未知区域。如果该节点位于空间边缘上，则变量为true，否则为false。举个例子，在一个地图上，一个机器人从起点A出发，当它到达一个新的点B时，如果点B附近有未知区域，那么这个点B的is_frontier就为true。如图蓝色为已达区域黄色点为frontier。

二者的区别：is_frontier表示的是空间边缘，这意味着它可以在地图的范围内，只是还没有被探索到。在一个机器人导航的场景中，当机器人探索到一个新的点时，如果这个点的附近有未知区域，那么这个点就可以被标记为边缘点。而is_boundary表示的是边界点，在地图的边缘。

是否为导航点is_navpoint：结合grph_planner.cpp的UpdateGoal函数理解。如图所示，移动过程中的局部导航点。

是否已经经过该节点is_covered（蓝色点）。

图描述字段：

uint32[] connect_nodes，所有相连的都连在一起（全局可见性图）。

uint32[] poly_connects，它可能表示该节点与多边形之间的连接关系。 "poly" 是多边形的意思。该数组中可能存储了该节点与其他节点相连接的多边形的编号。（看起来与connect_nodes几乎一致，后续研究区别）。

uint32[] contour_connects最好区分的，轮廓线：

uint32[] trajectory_connects，比上一个还简单的，轨迹连线：

接下来代码使用了received_graph变量，这暗示我们：

继续往下读会发现这个函数有些离谱，因为GraphDecoder::GraphCallBack的作用就是把"graph_msg"转化为received_graph而这个函数有把received_graph转换回去。再经过推敲可知这个节点应该不参加核心的逻辑，转换为received_graph仅仅是为了可视化和实现rviz按钮的回调函数。

而service操作貌似只是把订阅到的信息重新发布出去。如果真的是这样的话可以直接把这个信息也存成全局变量然后就可以不识闲Convert2Message和Encoder函数，直接读取内存并返回service要求的值提高速度。

至于作者为什么要实现左手倒右手再倒回来的操作，而不是直接把原始值放到内存里，希望通过阅读后续的代码获得理解。

全文读后补充：这种手法可能是用于多机协同联合建图，每个机器人都能通过这个节点获得所有机器人共建的图

far-planner节点 - 话题回调函数

第一行（far_planner.cpp第799行）没啥好说的，第二行出现FARMaster，研究一下这个类，以此为抓手，研究一下主节点。前面几个公有接口很平常。

接下来进入Init函数。

/reset_visibility_graph

第一行，reset_graph_sub_ = nh.subscribe("/reset_visibility_graph", 5, &FARMaster::ResetGraphCallBack, this);通过rqt_graph可知，这个是rviz发来的。

回调函数的调用关系如下图所示，最终的回调函数应该为void FARMaster::ResetEnvironmentAndGraph()。

void FARMaster::ResetEnvironmentAndGraph()

第一行就调用了一个函数（重设内部变量）：

接下来研究一下这几个变量的含义。

ResetInternalValues变量含义

odom_node_ptr_表示自身坐标对应节点

这个变量在src/far_planner/src/dynamic_graph.cpp中出现，其中CreateNavNodeFromPoint是根据robot_pos(Point3D类型)给odom_node_ptr_赋值，可见，这个NodePtr包含了机器人的自身坐标相关的信息。

is_planner_running_ 表示规划器在运行

该变量在src/far_planner/src/far_planner.cpp第276行被调用：

is_graph_init_ 只和 cout<<有关联

应该是字面意思，和主线逻辑关系不大，只和打印输出日志有关。

ClearTempMemory()

如图所示，根据调用的方式和代码的一贯风格，这几个应该全都是vector容器。接下来研究一下各变量的含义。

new_vertices_ptr_

超出了知识范围，在far_planner.cpp第69行初始化了该变量。是一个PCL类型

PointCloudPtr是PCL中定义的一种智能指针类型。它是pcl::PointCloud<PointT>类型点云数据的智能指针，用来管理内存，并且提供了一些便捷的操作。它可以简化点云数据的存储和操作。例如，PointCloudPtr类型的指针可以自动管理指向的点云数据的内存，在指针不再使用时自动释放内存。在PCL库中，PointCloudPtr类型用来存储pcl::PointCloud<PointT>类型的点云数据，PointT是点类型，可以是PCLPoint类型或其他类型。

这句话的意思是定义了一个指向pcl::PointCloud<PCLPoint>类型的智能指针new_vertices_ptr_。简单来说，就是定义了一个指向点云数据的指针，并且它是一个智能指针，可以自动管理内存。

pcl库入门

为了能够阅读PCL源码更好的学习相关知识，我通过源码编译安装了PCL库，大致了解后写了一个小项目。

接下来回顾这句话，它的的含义是如下第一行代码，因为point_struct.h文件定义了如下第二行代码：

接下来我们想详细看一下这个头文件(point_struct.h)。

struct Point3D

像类一样用结构体

该结构体中定义了大量函数。C++语法好奇怪，结构体还能当函数用。这种结构和类类似，不过默认公有继承、共有属性。并且可以用于初始化STL（比如在结构体重自定义哈希和比较）。结构体中首先初始化了一些构造函数

这段代码重载了包括PCL和eigen在内的各种类型的输入，可能这里他们要多人开发？

这里重载了等于和不等于运算法，他们没有直接用==而是使用了二者的差和一个很小的值比较。后来查了一下：在 C++ 中，使用 == 来判断两个浮点数是否相等是不可靠的，因为浮点数存在精度误差。正确的做法就是将两个浮点数的差与一个很小的数比较。

之后又重载了加减乘除运算法。

运算符重载涉及到一个C++知识点，常量函数：语法中的第二个const表示函数不会改变类中变量的值（常函数）。

hypotenuses（斜边），C++标准库函数之一，计算sqrt(a^2 + b^2)：

接下来定义了normalize、normalize_flat（这里作者的含义是归一化）。接下来又学到一个C++知识点：std::to_string()

接下来用一个友元操作<<运算法，看来struct和类几乎一模一样！接下来又定义了point_hash结构体。不过结构体里只有一个函数，没讲过这个语法，接下来对其展开研究。

新语法探索

写个小demo测试第一个例子所学，输出符合预期：

要弄清楚第二个函数做了什么需要弄明白重载()操作符、boost库的hash_combine、struct 包裹函数具体有什么意义。

重载()操作符相当于实现一个函数，这样之后对象(const Point3D &p) 就能return想要的值。具体如何获得可以在{}内定义。

hash_combine: https://zhuanlan.zhihu.com/p/574573421可以用它实现求类（结构体）的哈希。

struct 包裹函数。整个工程都没使用。但是经过研究，应该是和unordered_map联合使用，猜测如下：

剩下的那两个函数也比较无语，因为前面已经重载了==，又实现了相同的功能。这个point_struct.h定义很冗余且这个文件在多个功能包出现，且有一些功能整个工程都没有使用。应该是多个工程复用的文件，FastLAB自己定义的数据类型，方便各种格式操作。

该函数剩余内容

new_nodes_为第二行，该数据类型为NavNodePtr，字面意思应该是新发现的点。nav_graph near_nav_graph new_nodes clear_nodes都是NodePtrStack型。而CTNode则不同：

CTNode

对其进行合理猜测：

这个结构体是表示一个地图节点的信息。

Point3D position：表示节点的三维坐标

bool is_global_match：表示节点是否进行了全局匹配

bool is_contour_necessary：表示节点是否在轮廓线上

bool is_ground_associate：表示节点是否与地面有关联

std::size_t nav_node_id：表示节点在路径规划中的编号

NodeFreeDirect free_direct：表示节点的类型（凸点、凹点、柱点）

PointPair surf_dirs：表示节点的表面方向

PolygonPtr poly_ptr：表示节点所属的多边形

std::shared_ptr<CTNode> front：表示节点的前面节点

std::shared_ptr<CTNode> back：表示节点的后面节点

std::vector<std::shared_ptr<CTNode>> connect_nodes：表示节点与其他节点的连接关系

而最后一个结构realworld_contour_.clear();的定义为：

PointStack应该是拐角的几个点。将他们压成一个vector就是realworld_counter_。后面的内容根据字面意思就能看懂具体等主逻辑时再展开。

void FARMaster::OdomCallBack(const nav_msgs::OdometryConstPtr& msg)

本准备在终端中打印消息看下格式，但是没订阅到这个话题。看了一下far_planner.launch发现如下代码，据此找到了消息。

通过终端命令可以看到其消息格式为nav_msgs/Odometry。这个函数接收到odometry信息时会被调用。首先会将odometry信息从odometry坐标系转换到地图坐标系，然后更新机器人的位置和朝向，并将信息存储在robot_pos_和robot_heading_中。在第一次接收到odometry信息时，还会更新地图的起点位置。具体而言是，它读取了一个nav_msgs/Odometry 消息，其中包含了机器人在odom 坐标系中的位置和姿态信息。然后使用tf::poseMsgToTF() 将这个消息转换成tf::Pose 类型。接下来判断odom 坐标系是否与世界坐标系相同，如果不同就使用tf::TransformListener 获取odom 到世界坐标系的转换矩阵，再将获取到的tf_odom_pose乘上转换矩阵。然后利用这个tf::Pose 得到机器人在世界坐标系中的位置和姿态信息，并存储到robot_pos_ 和robot_heading_ 中。

void FARMaster::TerrainCallBack(const sensor_msgs::PointCloud2ConstPtr& pc)

翻译一下函数名：地形回调函数。这个函数的传递关系很清晰：

功能：当接收到某个topic的消息时就会被调用。这个函数的作用是处理点云数据，更新地图数据。具体来说，它会转换odometry坐标系到世界坐标系，更新机器人位置。然后对点云数据进行处理，分离出障碍物点云和空闲点云，更新地图栅格。检测是否有新的动态障碍物，更新点云栈和kd-tree数据结构。

先研究一下这个函数的输入：const sensor_msgs::PointCloud2ConstPtr& pc。pc由如下几部分构成：

header不必说。

height和widtth，想必和pcl::PointCloud同名参数的含义相同。

fields：点云数据的字段信息，即每个点包含哪些信息。

is_bigendian：点云数据的字节序。数据的存储可以采用两种不同的顺序：大端(big endian)和小端(little endian)。对于is_bigendian字段，如果它的值为true，那么点云数据就是采用大端顺序存储的；如果它的值为false，那么点云数据就是采用小端顺序存储的。在ROS中，sensor_msgs::PointCloud2类型的点云数据默认采用小端

point_step：每个点的大小。

row_step：每一行的大小。point_step：每个点所占的字节数。例如，如果点云数据中每个点都包含3个float类型的坐标值，那么point_step的值就是3sizeof(float)=12字节。row_step：每一行所占的字节数。如果点云数据是一个二维矩阵，那么row_step就是每一行的字节数。通常是point_step width。

data：点云数据的二进制表示。

is_dense：点云数据是否密集，即是否有缺失的点。

精彩！ UpdateRobotPosition(FARUtil::robot_pos);

读后总结：这个函数作用就是更新机器人在栅格地图中的坐标，并且把这个坐标更新为原点。后面去搞栅格位置换算、点云栅格的索引都是以他为原点的，所以必须把这个函数放到第一步。此外他还更新了neighbor_free_indices_，这一变量在后续经常被使用。

if (!is_init_) this->SetMapOrigin(odom_pos);默认是false，先看后面，一会再回来。

Pos2Sub的代码如下。

输入：机器人xyz坐标

输出：Eigen::Vector3i类型。表示机器人所在空间栅格的索引。

resolution_inv_是一个Eigen::Vector3d类型的变量，它存储了每个维度的逆分辨率。分辨率是指网格中每个单元格的大小。逆分辨率就是单位长度与单元格大小之间的比值。例如，如果网格中的每个单元格都是1米×1米×1米，则分辨率为1米。那么逆分辨率就是1。如果每个单元格是0.5米×0.5米×0.5米，则分辨率为0.5米，逆分辨率为2。

在这个函数中，resolution_inv_ 变量用来将空间中的位置转换为网格中的子单元格索引。它通过将空间坐标与网格原点的差值乘以逆分辨率来实现这一目的。这样，结果就是一个网格中的子单元格索引。

举个例子，假设我们有一个3D网格，每个单元格的大小是1m x 1m x 1m，原点是(0,0,0)。然后, 如果我们传入了一个向量(1.5,2.5,3.5)，那么返回的结果就是(1,2,3) ,即在网格中对应的子单元格索引。

接下应该是要计算邻居的位置，先把vector清空。

对应的数据结构：

这两行是设置“邻居”的范围。只有距离的格子的个数在这个以内的才计入。

将要探索的坐标传入这个函数。

为了看懂这个函数需要了解：在C++中, &= 是逻辑与赋值运算符，它将左操作数和右操作数的按位与的结果赋值给左操作数。&= 的运算过程类似于下面这个等价的代码：

size_是Grid初始化函数传入的参数，应该表示地图的范围。

最后获得的是neighbor_obs_indices_和neighbor_free_indices_。其中neighbor_free_indices_是同一高度上的点，neighbor_obs_indices_是一个体，将周围点的索引拿到。

为什么free只判断了一个平面？

这个问题我看完processcloud才反应过来，这里作者用了一个trick，那个平面是地面。（是不是有点太简单粗暴了，感觉在高低起伏的地面很容易误识别诶）。（如果在崎岖地面机器人与地面不平行的话）

最后一行调用是：

先顾名思义，它的意思应该是设置地形高度网格的起点(高飞称其为2.5D栅格)。（但是貌似这个命名和它代码关系不大诶）

为什么是这个更新公式？

这个公式是计算网格在x轴上的起点坐标,其中:

robot_pos.x :是机器人在x轴上的位置, 作为网格在x轴上的中心

res.x() :是网格在x轴上的分辨率

dim.x():是网格在x轴上的尺寸

根据这个公式，网格起点的x坐标计算如下：

首先, 按照网格的分辨率算出网格的总长度 res.x() * dim.x()

然后,将总长度除以2得到中心位置 (res.x() * dim.x()) / 2.0f

最后，将机器人的位置(robot_pos.x)减去中心位置，得到网格在x轴上的起点坐标

这样计算出来的起点坐标保证了网格中心与机器人位置重合, 这样可以保证网格能够在机器人移动时随之移动,始终以机器人位置为中心。

总结一下，map_handler_.UpdateRobotPosition(FARUtil::robot_pos);这个函数作用就是更新机器人在栅格地图中的坐标。

PrcocessCloud(pc, temp_cloud_ptr_);

读后总结：过滤，每个栅格只保留一个点，过滤Inf、Nan。

这个函数被多次调用。

输入：

typedef boost::shared_ptr< ::sensor_msgs::PointCloud2 const> PointCloud2ConstPtr pc，ros的消息类型点云数据。

输出：

typedef pcl::PointCloud<PCLPoint>::Ptr PointCloudPtr cloudOut 普通的PCL类

先赋值过去，如果为空直接return（return的也为空）。

FilterCloud(cloudOut, master_params_.voxel_dim);

给出这个函数的主重载：（ps：重载原来不用重新实现，可以来回调用。作者用了好多次，看了这个工程我才意识到）

这个函数主要使用了PCL库中的VoxelGrid类，这个类能够实现点云的滤波，它能够根据给定的参数将点云分割成若干个象元，并将象元内的点压缩成一个代表点。（就是稀疏化，反正最后是栅格地图，有一个点就行，这样计算开销小）

RemoveNanInfPoints(cloudOut)

这个函数的主要作用是从给定的点云中移除 NaN (Not a Number) 和 Inf (Infinity) 点。

它的主要步骤如下:

首先, 它创建了一个名为 temp_cloud 的 pcl::PointCloud<PCLPoint> 类型的变量,并设置它的大小为 cloudInOut 点云的大小。

然后, 它遍历 cloudInOut 中的所有点,对于每个点,如果该点的 x,y,z 坐标中有一个不是有限数（NaN或者Inf），就会打印一个警告，并且跳过这个点，否则就会将该点加入到 temp_cloud 中。

最后，它将 temp_cloud 的大小设置为 idx,表示有效点的个数，并将 temp_cloud 赋值给 cloudInOut 。

这个函数能够确保给定的点云中不会出现 NaN 和 Inf 点，这样能够避免在后续处理中出现错误。操作过于简单就不贴代码了。

CropBoxCloud(temp_cloud_ptr_, robot_pos_, Point3D(master_params_.terrain_range, master_params_.terrain_range, FARUtil::kTolerZ));

跟上一个函数功能差不多，都是封装一下PCL的功能，这个是裁剪电云，只保留给定范围内的。查了一下多余的那个参数的含义：min_vec 和 max_vec 是 Eigen::Vector4f 类型的变量，它们代表了一个四维向量，其中 x,y,z 是三维空间中点的坐标，w 是一个额外的值，通常用于附加信息。在这个函数中,最后一个值 1.0f 被用于设置 w 值，主要是为了满足 Eigen::Vector4f 的初始化要求并没有特别的意义。可以看到temp_cloud_ptr_再次被更新，这次只剩下机器人附近的点了。

ExtractFreeAndObsCloud(temp_cloud_ptr_, temp_free_ptr_, temp_obs_ptr_)

这个函数的逻辑也是极其简单没有什么可解释的。唯一要研究的就是判别条件：p.intensity < FARUtil::kFreeZ。它遍历了新点云中的所有点,对于每个点,它检查点的强度值是否小于FARUtil::kFreeZ。如果小于则说明该点是自由点,将其加入自由点云中，如果大于等于，则说明该点是障碍点,将其加入障碍点云中。结合上述信息，intensity就是代表对该点表示障碍物的置信程度。

事后补充：intensity在ExtractNewObsPointCloud中出现。

if (!master_params_.is_static_env)

MapHandler::UpdateObsCloudGrid(const PointCloudPtr& obsCloudInOut)

这个函数涉及到的数据结构极其复杂，并且是两个逻辑线交织在一起，我足足看了一个小时。这两个逻辑线，先说第一个：我将这个函数分为如下两个代码段，上段表示筛选，下段表示操作。要非常注意的是这里用的全是全局地图，我一开始没注意到变量名的细微差别理解偏差了很多。我直接把详细的注释写到了代码里：

遍历过滤后的obsCloudInOut（附近的点中大概率是障碍的），先把点云转化为栅格world_obs_cloud_grid_->Pos2Sub(Eigen::Vector3d(point.x, point.y, point.z));接着判断栅格是不是InRange（注意局部和全局），再获得它的index判断index是否在neighbor_obs_indices_中，如果全都满足，再进入下一步：

接下来是第二条逻辑线：

这三行代码单独拿出来看就很清晰了，随着机器人的移动，世界地图的栅格一定不与局部地图的栅格重合，所以世界地图的栅格并不是用点，而是每个点都是一个vector。所以每次更新都要重新再下采样一次。截至目前这个函数就很清晰了。使用局部地图更新全局地图。

MapHandler::UpdateFreeCloudGrid(const PointCloudPtr& freeCloudIn)

更新完障碍点还要更新自由空间的点。我们首先理解一下什么是free。如下图所示，被我设置成彩色的就是free，很显然，表示可以走的路。

接下来我们与研究一下其实现方法，和上一个函数完全一样，不必介绍。值得一提的是两个函数共同更新了global_visited_induces_，或许会在之后用到。

精彩！ ExtractNewObsPointCloud(temp_obs_ptr_, FARUtil::surround_obs_cloud_, FARUtil::cur_new_cloud_);

这个函数最后面非常精彩，废了好多脑细胞才把作者的思路逆向工程出来。原来intensity是这么用的！

先重设Intensity，刚刚一直在操作的temp_obs_ptr_的Intensity全部清零。

cloudRefer的产生如下：

现在我们把输入搞懂了，接下来遇到一个神奇的语法。经过假设验证得到其含义：

接下来又过滤一波得到新的点云数据。这波操作非常精彩：

把新旧点云加在一起加在一起做过滤：

之后这波操作或许比较好像，但是把它逆向工程推断出作者在想什么属实是废了不少脑筋：cloudRefer的Intensity被设置为255是为了与cloudIn的Intensity区分开来，在这个函数里，cloudRefer是用来和cloudIn合并为temp_new_cloud的，而cloudRefer的点的Intensity值对这个过程没有影响（只要够大就行）。在cloudIn和cloudRefer合并为temp_new_cloud之后，对于temp_new_cloud中的每一个点来说，如果它的intensity值小于FARUtil::kNewPIThred，说明这个点在cloudRefer中没有出现过，就是新的障碍物点，那么就会被加入到cloudNew中。

GetSurroundFreeCloud(FARUtil::surround_free_cloud_)

上回提到，UpdateRobotPosition很精彩，他不仅更新了原点还更新了neighbor_free_indices_，这不就用到了嘛！MapHandler::UpdateFreeCloudGrid(const PointCloudPtr& freeCloudIn)函数更新了world_free_cloud_grid_，这里直接从世界坐标里把附近的点抽出来。（简单粗暴哦，而且，他为什么不在合并之前做。效果一样但是这样看着逻辑有些乱。）

UpdateTerrainHeightGrid(FARUtil::surround_free_cloud_, terrain_height_ptr_);

顾名思义，作者要更新2.5D地图的高度啦，而且这个很显然要用到surround_free_cloud_。这个函数主要用来更新地形高度网格，它会对输入的自由点云进行降采样，并将降采样后的点云的高度值放入地形高度网格中。具体来说，首先会创建一个临时点云temp_free_cloud，将输入的freeCloudIn拷贝到temp_free_cloud中，然后将temp_free_cloud进行降采样。

接着对于每个temp_free_cloud中的点，将它与周围扩展的格子进行匹配，如果匹配上了就将该点的高度值加入到对应格子中。最后，会根据地形高度网格的信息更新障碍点云的周围格子的索引。

降采样，先复制输入再对拷贝的量做降采样。得到的降采样后的点云为copy_free_ptr

遍历copy_free_ptr中的point（PCLPoint&类型），对每个点进行扩展。将每个点（2D）扩展成一个3x3的网格（INFLATE_N的值为1）。

扩展过程，一个点变成了一个3x3的网格，依旧是2D的。扩展后的点存储在subs中。

接下来遍历，每个subs中的点，首先判断它是否Inrange，不Inrange的直接丢掉。接着获得它的Ind，如果terrain_grid_occupy_list_[Ind] = 0则表示它没有被计算过，进行如下计算。相当于给这个vector初始化一个点。如果不为0则表示该点已经初始化过，直接给他push_back一个新值。

现在已经获得了高度点云terrain_height_grid_，接下来要对这些点云一次判断可不可以走。如果可以走就会添加到terrainHeightOut中。

TraversableAnalysis(terrainHeightOut)

具体而言：这个函数用于分析点云的地形高度，确定哪些点可以被机器人穿过，哪些不能。它首先确定机器人的当前位置，然后使用一个lambda函数，IsTraversableNeighbor，来确定相邻点是否可以穿过。如果是，那么点的高度就会添加到terrainHeightOut点云中，并添加到可行走列表中。该函数使用广度优先搜索，从机器人的位置开始检查所有相邻点。

必要的注释：

这里dx dy要竖着看，一列为一个方向，就是前后左右四个方向。

这里visited_set突然出现可能会不明白他在作什么，实际是在做广度优先搜索遍历周围的点。

更新当前点所在的地形的高度。它遍历当前点的所有高度值（一个xy所对应的z的vector），并计算这些值与当前机器人位置高度差的绝对值，如果差值小于阈值H_THRED，则将这个值累加到平均值中，并累加计数器。当计数器大于0时，说明有有效的高度值，平均值就是该位置地形高度，将这个点加入到terrainHeightOut点云中。

AssignFlatTerrainCloud(terrainHeightOut, flat_terrain_cloud_);

这个函数的操作非常简单，仅仅是做了一个参数的转移，具体来说就是把输入的点pcl_p.intensity = pcl_p.z, pcl_p.z = 0.0f; 然后输出。

ObsNeighborCloudWithTerrain(neighbor_obs_indices_, extend_obs_indices_);

UpdateObsCloudGrid等函数会用到neighbor_obs_indices_，而extend_obs_indices_也会在不久后用到，这个函数的最后一步就更新这两个参数。（far-planer节点的UpdateGlobalNearNode的isNavPointOnTerrainNeighbor文档中会有更详细的介绍）其workFLow如下：

如图，最后剩下neighbor_obs就是粉色的点，因为之后在这个高度范围内的障碍物才会对轨迹规划产生影响，高度差太大不予考虑。

GetSurroundObsCloud(const PointCloudPtr& obsCloudOut)

这个函数很简单，根据上面获得的索引获得3D点加到obsCloudOut里，不必多说。下一段代码cur_dyobs_cloud_部分，我们方针环境中没有动态障碍物，无法调试所以暂时不介绍，时间允许的话我读下方针环境和局部规划的功能包再讲解一下这里。

总结TerrainCallBack回调函数

TerrainCallBack回调函数非常庞大，经过上述分析其输入量和整体逻辑为：

随着时间流逝他会自动更新去除旧点：

整体逻辑流程图：

void FARMaster::ScanCallBack(const sensor_msgs::PointCloud2ConstPtr& scan_pc)

源码中写订阅话题为scan_cloud，再结合rviz的图像可以得知该函数的输入是最最原始的雷达点云。函数逻辑很简单涉及到的函数之前说过，且并没有调用所以不做讲解。

void FARMaster::WaypointCallBack(const geometry_msgs::PointStamped& route_goal)

这个回调函数用一个Point3D承接goal，经过一些零碎的处理，最核心的是这一行代码：

UpdateGoal(goal_p)

这个函数是更新目标的函数。它首先重置目标，然后在当前图上查找最近的内部导航点，如果找到，则将其设置为目标，并标记为使用内部目标。

回顾一下什么是nav_node，如下图所示绿色表示走过轨迹，黄色表示nav_node。

如果没有找到，则创建一个新的导航点作为目标，并将其添加到图中。然后更新一些标志位，并记录目标的位置。最后，如果不是多层图，它会将目标的高度设置为目标附近的地形高度。接着更新graph_planner中的free_terrain_grid_的原点，将其设置为目标点。具体为什么这么设置我在 UpdateRobotPosition(FARUtil::robot_pos);一节中讲过。

void FARMaster::TerrainLocalCallBack(const sensor_msgs::PointCloud2ConstPtr& pc)

一字之差要比TerrainCallBack简单不少。我们先看一看二者的输入有何不同。首先从rqt上看，二者最直观的差别是TerrainCallBack函数的输入是本函数输入经过terrainAnaliysisExt处理后的结果。

根据rviz显示的结果来看，相比于ext有“延时效果”，它更像是实时效果。如果我们打开雷达图进行对比，可以猜测它很有可能是对传感器数据进行了一定的截断，而ext则很有可能是多帧时间指数衰减赋权加和。

不过这个函数依旧一直没有被调用，我们再结合上一个没有被调用的函数，可以推测出其调用逻辑像是之前讲的函数的简化版。根据函数内容倒推，可以看出框出来的变量应该是指是否需要自主建立地图。如果已经事先有全局地图了则不需要自己搞栅格高度不断滤波降采样。只需要做一个简单的匹配即可。

far-planner节点 Loop主函数

总体流程：

前20行都是一些工程上的东西，很有借鉴价值，后面这行代码调用了一个不明觉厉的类，研究一下：

class TimeMeasure

TimeMeasure是一个用于测量代码运行时间的类。它使用C++标准库中的chrono库来计时。类中有一个私有成员变量timer_stack_，是一个unordered_map类型，用于存储计时器的名称和开始时间。

类中提供了三个函数接口：

start_time(const string& timer_name, const bool& is_reset=false)：开始计时。函数接受一个字符串参数timer_name，表示计时器的名称，和一个布尔参数is_reset，表示是否重置计时器。

end_time(const string& timer_name, const bool& is_output=true)：结束计时并返回运行时间。函数接受一个字符串参数timer_name，表示计时器的名称，和一个布尔参数is_output，表示是否在终端输出时间。

record_time(const string& timer_name)：返回运行时间。函数接受一个字符串参数timer_name，表示计时器的名称。

通过在程序中使用这个类，可以很方便地测量代码运行时间，帮助程序员优化程序性能。学习了一下这个库，学到的新知识如下：

BuildTerrainImgAndExtractContour(odom_node_ptr_, FARUtil::surround_obs_cloud_, realworld_contour_);

输入：

自身的位置

周围的障碍点云

输出：

realworl_contour

调用UpdateOdom更新当前的姿态位置并将其分配给类成员odom_pos_。并通过free_odom_resized_ = ConvertPoint3DToCVPoint(FARUtil::free_odom_p, odom_pos_, true);函数将3D坐标转换为opencv类型的坐标（栅格地图的索引）。

调用ResetImgMat将类成员img_mat_重置为大小为MAT_SIZE x MAT_SIZE的单通道类型为CV_32FC1的零矩阵。

调用UpdateImgMatWithCloud将给定点云更新到img_mat_中，通过将点云中的每个点分配给img_mat_中的相应像素并增加该像素的值。

创建变量row_idx,col_idx,inf_row,和inf_col来存储图像中像素的索引。

（根据后续的操作inf_应该是inflate）

它创建了一个常量向量inflate_vec,包含-1,0,和1的值.

它遍历输入点云中的每个点并调用PointToImgSub函数将点从点云转换为图像中的像素坐标

如果像素坐标在图像范围内，它会增加以点为中心的3x3像素正方形的值1.0。

如果环境不是静态的，它会对图像应用阈值，使得任何值小于 cd_params_.kThredValue 的像素都被设置为0

如果 cd_params_.is_save_img 为真，它会调用SaveCurrentImg函数来保存当前的图片。

总之这两行代码的功能就是将周围的障碍物点云压缩到一张图片中（估计后面要opencv做角点检测啥的）。而且在压缩的时候还增加了模糊操作（每个点云对应一个3x3的图片格子）。

将3D点压缩+双重模糊弄成2D的好处是融合了多层的障碍信息。如图所示如果使用单线建立多边形则多半会只框出“树干”进而导致撞树。

调用ExtractContourFromImg从img_mat_中提取轮廓，

如果cd_params_.is_save_img为真,它会保存图片。

它将精细化的轮廓存储在PointStack的realworld_contour向量中。

根据上述信息可以推测出filtered_contours.size() = 图片中轮廓的个数：

这张图表示每个轮廓所对应的点集。

UpdateContourGraph(odom_node_ptr_, realworld_contour_)

经过这个函数ContourGraph::contour_graph_局部图中点的属性就被添加齐全了（除了z）。

ct_nodes就是可以用于图上的点。（v-graph上的点必要条件是ct_nodes）。

这个函数的功能是更新轮廓图。它首先清空原来的轮廓图，然后根据输入的过滤后的轮廓（filtered_contours）创建新的多边形，并将这些多边形添加到轮廓多边形中，并为这些点建立连线。

如果是Pillownode则根据论文不会做一些处理：

CreatePolygon(poly, new_poly_ptr);

对于每个多边形，它会更新机器人的自由位置（odom_free_position_），判断机器人是否在多边形内部。如果机器人在内部那肯定不是轮廓啦。接着判断多边形是不是柱子（直接机翻pillar），如果不是柱子，则会在每个多边形的顶点处创建轮廓点，并在这些轮廓点之间建立联系。至于判断的方法非常的朴素：如果是只有两个点，那必定连不成多边形，就是柱子。如果周长过小那可能是噪声，把它剔除出去（这个阈值有些暴力但我也没想到更好的方法。）（不过重新想一想如果我来实现我貌似不会再过滤一次，这样对比作者设置阈值过滤是比较合理的。）

判断点是否在多边形内

这个代码的基本思想是，从给定点向任意方向发射一条射线，如果该点在多边形内，那么射线与多边形的交点数量应该是奇数；如果该点在多边形外，那么射线与多边形的交点数量应该是偶数。使用了一个for循环来遍历多边形的每一条边，并判断给定点与该边的交点是否存在。具体来说，首先它判断给定点y是否在当前边的端点之间，如果是，它进一步判断给定点x是否在射线与当前边的交点的左侧。如果是，它会将计数器c取反。这里的判断给定点x是否在射线与当前边的交点的左侧，是根据交点的坐标来判断的。在二维平面上，如果一个点在另一个点左边，那么它的横坐标应该比另一个点的横坐标小。因此，这里使用了一个判断式 x < (xp[j] - xp[i]) * (y - yp[i]) / (yp[j] - yp[i]) + xp[i] 来判断给定点x是否在射线与当前边的交点的左侧。

类型转换，从PointStack变成PolygonPtr，不过核心数据使用智能指针存的，减小开销。

UpdateOdomFreePosition(odom_node_ptr_, FARUtil::free_odom_p)

从这里可以推测出free的含义。free表示机器人是否可以触及。

我们把这个函数的输入类型分为两类，一类是没有触发上图红框框出的内容，其主逻辑如下图所示，几乎没什么操作，就直接赋值，无需解释。运行完标红的代码就直接return。如果触发了那个if则在点周围扩展一个矩阵，在矩阵中找满足条件的最优点（根据indensity排序，free的indensity从0开始操作，obs的indensity从255开始操作，具体在一个回调函数中说过）。

在这只有又是一段很复杂的操作。下面第一段代码对应论文中的

判断这个poly是不是is_pillar的，如果是的话就说明要么它确实是只有两个点的柱子，要么就是有多个点但相距非常的近。对此我们采用的方法是直接将他们平均，然后AddCTNodeToGraph。

接下来对于正常的点，先建立一个环然后将这个环加入到polys_ctnodes_。至于AddCTNodeToGraph，这一点和上面一致，唯一不同的是上面加入平均值，这里加入全部。

AnalysisSurfAngleAndConvexity(ContourGraph::contour_graph_);

最后，这个函数会分析轮廓图上每个节点的表面角度和凸性。上一个函数满足要求的点和不满足要求但取了平均之后的点都会被加入到contour_graph_，而正是这个函数的输入和输出。此处对应论文如下部分：

首先看前几行，这里最有用的操作就是ctnode_ptr->surf_dirs = {Point3D(0,0,-1), Point3D(0,0,-1)}; 之后这行代码都会和注释"This Node should be a pillar."一同出现。

接下来对于第一批筛选的获胜者，进行第二轮筛选。第一轮的获胜者一定是一个环，接下来作者绕着这个环转一圈，如果有两个节点的距离太小那就把这个环干掉。

接下来进入ContourSurfDirs函数，该函数接受两个向量返回一个单位向量。surf_dirs就变成一个单位向量了。正转一圈反转一圈，surf_dirs的first和sccond就是两个方向向量（这里和之前猜测的不一样）。所以截至目前一共已经经历了三轮淘汰，经过这三轮淘汰后的胜利者进入AnalysisConvexityOfCTNode(ctnode_ptr);迎接下一轮淘汰。

这个函数首先判断是否是PILLAR，此处没什么营养跳过。

接下来判断该点是个棱角还是个墙（平的），如果是棱角的话就返回拓扑方向（角分线的方向）如果is_wall的话就ctnode_ptr->free_direct = NodeFreeDirect::UNKNOW;如果是个棱角就判断是convex的还是concave的

判断凸凹性

再次惊叹，判断凸凹的算法一开始还没看懂，那张纸比划比划后豁然开朗，实在是太妙了：

AdjustCTNodeHeight(ContourGraph::contour_graph_)、AdjustNodesHeight(nav_graph_)

变量说明：nav_graph_是全局地图contour_graph_是局部地图

根据地形图更新contour_graph_的高度和nav_graph的高度。

UpdateGlobalNearNodes()

参见https://blog.csdn.net/weixin_39977764/article/details/123895386。这个函数最初没看懂，因为没搞懂is_active的含义是什么，请教了CSDN上的博客作者。

GetExtendLocalNode()

只有一行代码，return 一个属性。（也不知道为啥不直接设成公有）。

返回DynamicGraph::extend_match_nodes_，是刚才所说的near_nav_nodes_的超集，可以覆盖要匹配的点。

MatchContourWithNavGraph(nav_graph_, near_nav_graph_, new_ctnodes_)

输入：

nav_graph_ 全局地图

near_nav_graph_全局地图中可能在局部地图出现的点

类自带的属性，counter_graph_局部地图（没有写在函数参数里）

输出：

new_ctnodes_

首先将要遍历的临近全局地图相关属性初始化：

接着为局部地图中合适的点找到最近的全局地图上的对应点，判断二者是否匹配（距离足够小、连线是否和多边形有碰撞）。通过之后将ctnode 的nav_node_id设置为全局地图上对应点的id。这样一个多边形上可能会有部分点和全局地图完成了匹配，匹配之后进入this->EnclosePolygonsCheck()函数（具体作用没搞清楚）。之后将局部地图上合适的点加入到new_ctnodes_。

之后调用graph_manager_.ExtractGraphNodes(new_ctnodes_)将ctnodes中的局内点加入图中。

规划主函数FARMaster::PlanningCallBack

void FARMaster::PlanningCallBack(const ros::TimerEvent& event)

这是一个ROS定时器的回调函数。该函数首先检查图是否已经初始化，如果没有则返回。然后，它检查是否存在目标节点，如果不存在，则调用UpdateGraphTraverability更新图的可通过性并返回。

void GraphPlanner::UpdateGraphTraverability(const NavNodePtr& odom_node_ptr, const NavNodePtr& goal_ptr)

该函数用于路径“预规划”。首先，它初始化所有节点的状态。然后，它从当前位置出发，使用迪杰斯特拉算法扩展整个图，以当前里程计节点为根建立全图范围内的导航树。

首先进行异常检测：

对图上各节点的状态进行初始化：

node_ptr->gscore 是该节点的g值，表示从起点到该节点的估计代价。

node_ptr->fgscore 是该节点的fg值，表示从起点到该节点的自由空间估计代价。

两者的区别是gscore是在导航图上搜索的代价，而fgscore是在空闲空间上搜索的代价。在这里，gscore 用于记录起点到当前节点在导航图上的估计代价，fgscore 用于记录起点到当前节点在空闲空间上的估计代价。

node_ptr->is_traversable 是该节点是否可以从当前位置到达。

node_ptr->is_free_traversable 是该节点是否可以在空闲空间中到达。

node_ptr->parent 是该节点的父节点，即从起点到该节点最短路径上的上一个节点。

node_ptr->free_parent 是该节点在空闲空间中的父节点，即从起点到该节点最短路径上的上一个节点。

parent 和 free_parent 都是用来记录起点到该节点最短路径上的上一个节点。 parent是在导航图上的父节点，当规划的路径是在导航图上的时候，使用parent。 free_parent是在空闲空间上的父节点，当规划的路径是在空闲空间上的时候，使用free_parent。

所有节点的 gscore, fgscore 都初始化为无穷大，表示初始时尚未进行搜索。is_traversable, is_free_traversable 都初始化为 false, 表示初始时都不可以从当前位置到达。parent, free_parent 都初始化为空指针，表示初始时都没有父节点。

接下来将odom的gscore设置为0（这个值应该是值当前点到目标的距离）

接下来初始化一个std::unordered_set<std::size_t> IdxSet。IdxSet open_set;。从数据结构（哈希表）可以看出这个表记录节点是否被访问过。（openset记录要访问的closeset记录访问过的）接下来创建优先级队列open_queue:

队列中的元素自小到大排序：

接下来是非常常规的图搜索操作：弹出节点，访问节点的邻居们。

如果邻居已经在close_set中说明已经访问过了，就不管了。对于不在close set中的邻居，计算和自己的欧拉距离。如果欧拉距离过大就再说。如果欧拉距离不算大则将当前节点的cost+到邻居的欧拉距离大于邻居自己的的cost则做如下操作：

这里是使用迪杰斯特拉算法获得到邻居点的最短路径。最短路径以树的形式存储。

void GraphPlanner::UpdateGoalNavNodeConnects

如果获得了目标点，则将目标点加入到图中并更新与其他点的连线。遍历每个节点，判断该节点是否为当前节点能够“够得到”的（前面判断的），是否不是被某个多边形挡住了。接下来如果二者都是“凸点相连”则将IsValidConnectToGoal返回true，如果目标距离太远不可见也返回true。对返回true的点展开投票：

void DynamicGraph::RecordPolygonVote、DeletePolygonVote

这个函数参数命名有些奇怪，这里根据调用它的代码做些注释：

首先找到对彼此的连线：

如果彼此之间没有连线就初始化一条连线。

如果有就给他投票，vote_queue1和vote_queue2后面push_back一个1，如果没有相连就给他push_back0。再基于此通过IsPolygonEdgeVoteTrue进行检查。queue_size是三，只要三帧中有两为1就返回true。它为true后再进行进一步检查，通过后将is_connect设置为true并返回。IsValidConnect会接受这个返回值如果为true就添加边如果为false就抹去边。经过上述步骤目标点及连线被加入到了图中。如图白线就是加入的目标点，不考虑未知空间，考虑“可视性遮挡”连上边。