Tare_planner的学习教程(五)
Tare_planner的学习笔记(五)
从今天开始，楼主进行tare_planner的主体算法的解析，首先在tare_planner的算法包中主要有17个头文件夹，按照头文件的引用关系进行算法部分的解析，首先是utils文件夹中文件的解析。
1.utils文件
utils文件夹中主要有两个头问价misc_utils.h和pointcloud_utils.h,首先介绍主要功能的文件misc_utils.h。
#define MY_ASSERT(val) \ if (!(val)) \ { \ std::cout << "\033[31m Error at [File: " << __FILE__ << "][Line: " << __LINE__ << "]" \<< "[Function: " << __FUNCTION__ << "] \033[0m\n"\<< std::endl; \ exit(1); \ }上述定义了一个预定义函数，主要是输出运行时出错的文件和函数主体。接着定义了一个misc_utils_ns命名空间。在命名空间之下定义了许多函数，下面针对这些函数进行具体的说明。
2. 模板函数
首先介绍其中定义的模板函数。
| 模板函数 | 模板函数说明 |
|---|---|
| PointToPoint | 实现了从两种类型的点的坐标的转换 |
| KeyposeToMap | 实现了点云信息到map坐标系的坐标转换 |
| PointXYDist | 实现了两个平面点之间的距离计算 |
| PointXYZDist | 实现了两个空间点之间的距离计算 |
| LinInterpPoints | 如果两点之间距离小于阈值，则存入interp_points中；如果两点之间距离大于阈值，则按照比例进行插值，将插值存入interp_points中 |
| InRange | 返回index的编号是否在list中的结果 |
| getParam | 实现nh.param读取参数服务器的功能 |
| PublishCloud | 将点云信息转换成ROS的消息进行发布的模板函数 |
| Publish | 发布ROS的msg的模板函数 |
3. Timer类
Timer类中只有三个函数Start、Stop和GetDuration。其中Start的功能是开始计时，Stop的功能是结束计时，GetDuration的功能是计算两者的时间差。
4.Marker类
Marker类中主要有6个函数
| 函数 | 函数说明 |
|---|---|
| Marker | 定义了ROS的接收话题和新建Marker |
| SetColorRGBA | 设置了Marker的颜色 |
| SetScale | 设置了Marker的比例 |
| SetType | 设置了Marker的类型 |
| SetAction | 设置了Marker的动作类型 |
| Publish | 发布了设置好的Marker的话题 |
5.其他函数
除此之外，楼主要介绍一下其余的功能函数
| 函数 | 函数说明 |
|---|---|
| PCL2GeoMsgPnt | 将PCL的点云信息转换为ROS的msg |
| GeoMsgPnt2PCL | 将ROS的msg转换为PCL的点云信息 |
| GeoMsgPoint | 将PCL的点转换为ROS的点msg信息 |
| PCLPoint | 将ROS的点msg转换为PCL的点 |
| LeftRotatePoint | 先绕y轴左侧旋转90度，在绕x轴左侧旋转90度的坐标转换 |
| RightRotatePoint | 先绕y轴右侧旋转90度，在绕x轴右侧旋转90度的坐标转换 |
| VectorXYAngle | 函数来计算xy平面上两个向量之间的角度，z分量被省略 |
| PointAngle | 函数计算机器人和目标点的方向 |
| CollinearXY | 函数用于检查xy平面中的三个点是否共线 |
| LineSegIntersect | 函数用于检查两条线段是否相交，如果线段“p1q1”和“p2q2”相交，则返回true，否则返回false |
| LineSegIntersectWithTolerance | 与LineSegIntersect()类似，只是添加了一个“公差”，使得每条线段的两端都延伸了该距离，这将比LineSegIntersect()更频繁地返回真值 |
| ThreePointOrientation | 函数查找有序三重态(p，q，r)的方向,0 --> p, q ,r共线, 1 --> 顺时针, 2 --> 逆时针 |
| PointOnLineSeg | 该函数主要是确定点是否在线段上。给定三个共线点p、q、r，函数检查点q是否位于线段“pr”上 |
| AngleOverlap | 函数计算两个角度间隔[s1，e1]和[s2，e2]的重叠。两个间隔的重叠。如果重叠，则>0，否则<0 |
| AngleDiff | 返回两个角度之间的差值 |
| PointInPolygon | 检测给定的点是否在多边形内部 |
| LineSegDistance2D | 函数获取从点到线段的距离，忽略z轴 |
| LineSegDistance3D | 函数获取从点到线段的距离 |
| DistancePoint2DToPolygon | 函数获取从一个点到多边形边界上最近点的距离 |
| LinInterpPoints | 如果两点之间距离小于阈值，则存入interp_points中；如果两点之间距离大于阈值，则按照比例进行插值，将插值存入interp_points中 |
| DegreeToRadian | 弧度到角度的转换 |
| RadianToDegree | 角度到弧度的转换 |
| ConcatenatePath | 将路径2连接到路径3之后 |
| SetDifference | 函数可以用来求两个集合的差集 |
| signum | x变量的正负值符号 |
| mod | 返回 (value % modulus + modulus) % modulus的值 |
| intbound | 求最小的正t，使s+t*ds为整数。 |
| InRange | 返回index的编号是否在list中的结果 |
| RayCast | 完成点的投影工作 |
| InFOV | 检测点是否在视角区域内部 |
| InFOVSimple | 简化了点在视角的检测 |
| ApproxAtan | Atan的大约计算 |
| ApproxAtan2 | Atan2的大约计算 |
| GetPathLength | 得到了路径的长度 |
| AStarSearch | a*算法得到路径 |
| AStarSearchWithMaxPathLength | 具有最大路径长度的a*搜索 |
| SimplifyPath | 简化得到的路径，在共线的路径点全部排除掉 |
| DeduplicatePath | 计算双向路径，确定路径的前进方向 |
| SampleLineSegments | 利用采样距离进行路径的切割 |
| UniquifyIntVector | 按照距离重新排序vector中的点集 |
6.pointcloud_utils
pointcloud_utils的文件中主要定义了三个类，都是对于点云数据进行处理的。第一个类是VerticalSurfaceExtractor,主要是利用点云信息处理垂直的表面，这对于地下空间探索有着重要的意义。第二个类是一个模板类PointCloudDownsizer,主要是完成点云数据的下采样。第三个类也是一个模板类，主要是完成PCL的点云处理。
7.VerticalSurfaceExtractor
VerticalSurfaceExtractor中设置了一下参数
double kRadiusThreshold; // KdTree搜索的距离阈值 double kZDiffMax; // 高度差的最大值 double kZDiffMin; // 高度差的最小值 int kNeighborThreshold; // 邻居判定的距离然后针对点云进行了处理，然后搜索到点云周边的最近邻，然后判断近邻点的高度，如果高度合适，并且近邻点的数量足够多，则判定为一个垂直面。
8.PointCloudDownsizer
PointCloudDownsizer主要定义了点云下采样的函数。
9.PCLCloud
PCLCloud的定义了点云的接收与发布的话题。
以上就是最为基础的文件夹的函数解析，如有错误请大家批评指正。感谢大家的支持，喜欢楼主的记得点赞关注哦！！！
