Tare_planner的学习教程(五)

马业

2023-12-01

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的定义了点云的接收与发布的话题。

以上就是最为基础的文件夹的函数解析，如有错误请大家批评指正。感谢大家的支持，喜欢楼主的记得点赞关注哦！！！