ROS octomap常用函数介绍和实时显示octomap
关于octomap的介绍就不在本文展开了，请移步高翔老师博客https://www.cnblogs.com/gaoxiang12/p/5041142.html
我这里就简单介绍一下octomap https://github.com/OctoMap/octomap 里的常用函数，方便大家在自己的程序里调用。
我具体使用场景是，需要在自己的程序中维护一个octomap并在rviz中实时显示（配合octomap rviz plug in）
1. 首先是建树，建议使用指针
//这是最简单的octomap，没有设置颜色，0.03是分辨率可以自己更改，单位米
octomap::OcTree* octomap = new octomap::ColorOcTree(0.03);//这是彩色octomap，0.03是分辨率可以自己更改，单位米
octomap::ColorOcTree* octomap = new octomap::OcTree(0.03);
2. 然后是插入点，这里有两种模式，一种是直接插入点，另一种是插入一条射线。注意，无论彩色还是无色octomap都需要先插入一个点，之后在给彩色octomap的这个点上色
插入点很好理解，就是在给定坐标位置添加一个node（或者已有node的概率升高）
//updateNode 是插入单个点，接受两个参数，第一个就是octomap::point3d类型的3d点，第二个参数默认填true即可
double x = 1;
double y = 2;
double z = 3;
octomap->updateNode(octomap::point3d(x, y, z), true);插入射线需要起始点和终点，在终点位置添加一个node（或者已有node的概率升高），在起始点到终点之间经过的node概率降低（可以认为起始点是观测点，终点是目标点，那么我既然观测到了目标点，说明目标点之前没有遮挡，如果有遮挡，可能是误差造成的，那就需要对这个遮挡点进行降权）
//insertRay 是插入射线，有两个参数，依次是起始点和终点，都是octomap::point3d类型的3d点
//例子是以（0，0，0）为起点，（1，2，3）为终点的射线
double x = 1;
double y = 2;
double z = 3;
octomap->insertRay(octomap::point3d(0, 0, 0), octomap::point3d(x, y, z));然后给彩色点上色
//给彩色octomap设定rgb颜色, 对应六个参数分别是目标点xyz颜色rgb
unsigned int R = 216;
unsigned int G = 120;
unsigned int B = 255;
octomap->setNodeColor(x, y, z, R, G, B);
3. 一些简单常用的函数
//清空octomap
octomap->clear()
//剪枝（原理见高翔老师博客）
octomap->prune()
4. 实时显示，需要安装插件octomap rviz plug in
octomap rviz plug in 接收 Octomap类型的message，我们需要把已有的octomap转化成message，也很简单
//声明advertise，octomap rviz plug in 默认接受topic为octomap_full的message
pub_octomap = n.advertise("octomap_full", 1, true);
//声明message
Octomap map_msg;
//设置header
map_msg.header.frame_id = 'world';
map_msg.header.stamp = ros::Time::now();
//fullMapToMsg负责转换成message
if (octomap_msgs::fullMapToMsg(*octomap, map_msg))
//转换成功，可以发布了
pub_octomap.publish(map_msg);
else
ROS_ERROR("Error serializing OctoMap"); 这位博主介绍了如何安装和启动plug in，不清楚的同学请移步https://blog.csdn.net/crp997576280/article/details/74605766
5. 这里只给了几个常用函数，简单的操作是够了，octomap还有一些其他函数，我暂时也没用到，有兴趣的可以去看一看代码https://github.com/OctoMap/octomap