fast-planner读取不到真实地图

问题描述

系统环境：Ubuntu16.04
硬件：双目相机intel D435i，机载电脑intel NUC
最终目标是实现视觉惯性导航的无人机自主飞行。

之前已经使用d435i跑通了vins-mono，跑通了fast-planner，但是在fast-planner打开的rviz窗口中，显示不到相机采集到的实际地图环境。
前期过程参考了

从零实现vins-mono+fast-planner+M100无人机实验在现实场景中的应用

解决过程

通过观察，在vins的终端中有这样的一段提示：
Client [/fast_planner_node] wants topic /vins_estimator/point_cloud to have datatype/md5sum [sensor_msgs/PointCloud2/1158d486dd51d683ce2f1be655c3c181], but our version has [sensor_msgs/PointCloud/d8e9c3f5afbdd8a130fd1d2763945fca]. Dropping connection.

Client [/fast_planner_node] wants topic /vins_estimator/point_cloud to have datatype/md5sum [sensor_msgs/PointCloud2/1158d486dd51d683ce2f1be655c3c181], but our version has [sensor_msgs/PointCloud/d8e9c3f5afbdd8a130fd1d2763945fca]. Dropping connection.

通过搜索，是由于vins输出的点云类型为sensor_msgs/PointCloud，但是fast-planner使用的点云输入类型为sensor_msgs/PointCloud2

之前使用过RTAB-MAP输出的点云类型是PointCloud2。
查看话题类型：

rostopic info /rtabmap/cloud_map

得到提示

Type: sensor_msgs/PointCloud2

Publishers: 
 * /rtabmap/rtabmap (http://uav2-NUC:38177/)

Subscribers: None

更改到fast-planner的launch文件中进行实验：

<arg name="odometry_topic" value="/rtabmap/odom"/> 
<arg name="cloud_topic" value="/rtabmap/cloud_map"/>

在fast-planner中可以看到生成的地图。

转换点云类型

如果想要继续使用vins-mono作为里程计，需要对输出的点云类型进行转换，把sensor_msgs/PointCloud转换为sensor_msgs/PointCloud2

github上有开源项目可以实现这一功能

point_cloud_converter

修改项目的launch文件

<remap from="points_in" to="/vins_estimator/point_cloud"/>
<remap from="points2_out" to="/vins_estimator/point_cloud2" />

即可发布一个类型为sensor_msgs/PointCloud2的节点**/vins_estimator/point_cloud2**