版权声明：本文为博主原创文章，未经博主允许不能随意转载。
本文链接：https://blog.csdn.net/AnChenliang_1002/article/details/109535355

最近由于科研的需要，要将VINS-mono与fast-planner结合并最终应用给到无人机中。下面记录实现的过程：

本人的设备：
Ubuntu 16.04系统，Intel realsense D435i相机，VINS-mono算法，fast-planner算法，DJI M100无人机。

1.用自己的摄像头D435i跑通vins-mono

实现过程参考：
从零开始使用Realsense D435i运行VINS-Mono

2.跑通fast-planner

参考我另一篇博客：
Fast-Planner安装、环境配置以及问题解决

3.VINS-mono与fast-planner结合

本节需要做的就是把VINS-mono输出的odometry话题和D435i相机发布的深度图像话题发布给fast-planner作为其里程计信息和深度图像信息的输入。

3.1 修改fast-planner的launch文件

打开fast-planner中的kino_replan.launch文件（路径为/home/acl/fast-planer_ws/src/Fast-Planner/fast_planner/plan_manage/launch其中acl为我的用户名，fast-planer_ws为我存放fast-planner的工作空间）:
step 1:
将 <arg name="odom_topic" value="/state_ukf/odom" />
改为 <arg name="odom_topic" value="/vins_wstimator/odometry" />
此为里程计信息输入话题，原版是/state_ukf/ofom，将其改为我的vins-mono的输出里程计的话题，即/vins_wstimator/odometry。
step 2:
将 <arg name="depth_topic" value="/pcl_render_node/depthdi"/>
改为 <arg name="depth_topic" value="/camera/depth/image_rect_raw"/>
此为深度图像信息话题，原版是/pcl_render_node/cloud，将其改为我使用的D435i相机发布的深度信息话题，即/camera/depth/image_rect_raw。

3.2 使用VINS-mono运行fast-planner

step 1:用D435i运行VINS-mono
打开第一个终端：

cd catkin_ws
source devel/setup.bash 
roslaunch realsense2_camera rs_camera.launch

打开第二个终端：

cd catkin_ws
source devel/setup.bash 
roslaunch vins_estimator realsense_color.launch

打开第三个终端：

cd catkin_ws
source devel/setup.bash 
roslaunch vins_estimator vins_rviz.launch

此时会发现vins-mono在D435i下的画面，晃动相机使其初始化完成。
step 2:运行fast-planner
打开第四个终端：

cd fast-planer_ws/
source devel/setup.bash
roslaunch plan_manage rviz.launch

如果此时发现，打开fast-planner的rviz后，VINS-mono的rviz被杀死，出现此问题的原因及解决办法见我另一篇博客：

解决同时启动两个rviz发生冲突的问题
然后打开第五个终端：

cd fast-planer_ws/
source devel/setup.bash
roslaunch plan_manage kino_replan.launch

step 3:修改fast-planner地图
在fast-planner的rviz中，点击左侧的Mapping的+号，将simulation_map取消勾选，将real_map勾选。
此时就能看到fast-planner在VINS-mono加持下的运行结果。如下图：

在这里插入图片描述
图中带颜色的圆柱形就是真实地图中的障碍物了。
自此已经实现了VINS-mono+fast-planner的组合，下一步就是将其应用在无人机上。

4.将VINS-mono+fast-planner应用的无人机上

首先附上fast-planner的节点框架图。从节点框架图中，所有节点和话题之间的关系都一目了然。
在打开fast-planner之后，重新打开一个终端运行：

rosrun rqt_graph rqt_graph

在这里插入图片描述
从红框中的部分可以看出fast_planner_node将/planning/replan话题发布给/traj_sever节点，traj_sever节点将/planning/pos_cmd发送给仿真控制器so3_cmd节点，so3cmd节点将控制指令/so3_cmd话题发送给无人机。