把ego-planner的plan_env单独取出订阅位姿和深度图构建自己可用的导航地图

修改后的plan_env功能包已经上传

GitHub - maxibooksiyi/ego_grid_map: 把ego-planner的gridmap拿出来当作一个功能包

https://gitee.com/maxibooksiyi/ego_grid_map

ego中的地图，可以看看下面这两篇博文，可以看出ego基于位姿融合深度图基本是上面那个思路。  
https://blog.csdn.net/chunchun2021/article/details/132761925  

https://blog.csdn.net/weixin_42284263/article/details/122283727

有人把ego-planner的gridmap单独弄成一个节点，这个很合我意。  
https://blog.csdn.net/chunchun2021/article/details/134535140  
我看了下地图这部分确实相当于一个单独的功能包，是可以单独拿出来的，现在理解fastlab的人说改为激光雷达点云耗时长的意思了把，就是更新这个占据栅格地图，耗时比较大会，可能不合适继续用占据栅格地图了，以及ray cast的计算也大，不过cartographer不也是ray cast么，可能它是二维的。  
https://gitee.com/maxibooksiyi/ego-planner/tree/master/src/planner/plan_env  

我单独把ego-planner的gridmap拿出来当作一个功能包，在一个工作空间下编译可以编译通过！！！


我为此专门新建一个仓库来存放此功能包  
https://gitee.com/maxibooksiyi/ego_grip_map  

我用densesurfelmapping给的bag包配合运行，因为里面有vins的位姿和对应深度图 查看节点图没有订阅任何话题，是不是我只调用了init函数

我看了下GridMap类的成员函数，于是加了一句 grid_map_test_->publishMap(); 现在配合bag包运行可以看到有地图相关话题发出来了。

打印/grid_map/occupancy_inflate和/grid_map/occupancy看着也有值，但是内容不多。

然后我看了下ego-planner里面是如何调用publishMap()的，似乎是在一个定时器的回调函数里面。而这个定时器的定义似乎就在GridMap::initMap(ros::NodeHandle &nh)里面，所以我似乎不必多写一句publishMap()，是的，我把我加的publishMap()注释了，运行launch文件，也有/grid_map/occupancy_inflate和/grid_map/occupancy话题。

那之前运行为什么话题没有订阅上呢，我再看了下launch文件，发现位姿态它要的odom，而你给的是Posestamped类型的话题（后来发现代码里可以选择订阅posestamped类型还是odom类型）

我看留下节点图，话题依旧没有订阅上，是不是我launch文件改的参数值没有生效，我看了下ros的参数，似乎没有odom和深度图话题

看了下代码也没有从launch文件读取odom话题和深度图话题，看来可能得直接代码里改这两个订阅的话题名称

代码里把订阅的话题名称改过来后，再起，看节点图话题就订阅上了！！！！！

打印/grid_map/occupancy话题也有很多值了

rviz查看也有了！！！！注意rviz的根坐标系需要选为world

颜色也可以选择彩色显示

把点的尺寸由5里面变为10厘米是不是看着就更有味道了！！！！！

改为/grid_map/occupancy话题查看

rviz里面坐标系要选为world也是launch文件这里frame_id选择的world

现在回看 关于ego-planner里面的GridMap_use_depth_filter_-CSDN博客 这个博客里面写的node节点没有问题！！！确实主要就调用个initMap(nh_) ; 就可以了，就是订阅话题的时和需要注意一下，在launch文件改是无效的。

但是目前还没有达到我想要的建图的效果，因为时间长的点似乎它会自动消失掉。 而且SLAM的稠密建图你可能还得考虑加上色彩。

这个launch文件里面很多参数还是值得研究一下的

把ego的plan