> 文档中心 > Far_planner 无聊系列(1)

Far_planner 无聊系列(1)

        心血来潮,做了一个自动pub goal的小插件,然后做了一个word_obs_cloud_grid remap到平面grid的映射,看起来是不是很像之前的栅格地图,没错,它就是~

首先需要建立一个新的cpp 叫做global_grid.cpp 参考之前的graph_planner.cpp就可以知道,我们需要取出word_obs_cloud_grid下的点云信息,即每个格子为5x5,而我们用到的grid比较细,是0.5x0.5的,如果需要按原始map进行采样,1000m就需要2000x2000个sub,所以我先选了按地图合适的尺寸,就是600x600的样子。

主要是一些初始化grid,设置grid的origin坐标,因为不能按小车的odom更新,否则会出现点云的漂移,所以需要人为定origin,我把这一个当做参数写到yaml里了,调试的话就很方便。

        

        这一部分和graph_planner也差不多,只是不再对grid进行清空而已,因为我们需要之前的信息。

         然后是生成,这里就按各自的需要了,我还要进行下一步的研究,所以导出的是0和1和2这样的数字数组,如果不需要,也可以在在这一步直接进行opencv的显示。

         在far_planner主函数里加入这个部分,is_out_grid作为param写到yaml里了,为了不占据计算v-graph的时间,我把它写在了loop的最后。


        显示的部分我用python写的,没有写在far_planner主函数里是因为我不需要在主函数调用这个,我只是用来判断我输出的data对不对而已罢了,如果有需要的可以直接写在主程序里。

         arr_size主要是用来把list给分块的,因为我的最小单位是0.5,地图尺度最大是1000m,那这样最多就会有2000x2000这样的矩阵出现,即[[....],[...],[...]......[...]]

        openfile主要就是读取我的data数据,把它按照image的要求来进行读取