D435i+vins-Fusion+ego-planner+yolo无人机避障实测

vins-Fusion部分更改

1、D435i相机文件修改
修改~/catkin_ws/src/realsense-ros/launch/目录下的rs_camera.launch
重新新建重命名为rs_camera_vins.launch，将它保存在同目录下，修改内容主要有：

<arg name="enable_gyro"         default="true"/>
<arg name="enable_accel"        default="true"/>
//联合方式copy或linear_interpolation
<arg name="unite_imu_method"          default="linear_interpolation"/>
//时间戳同步
<arg name="enable_sync"               default="true"/>

2、Vins-Fusion修改
修改~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/…目录下realsense_stereo_imu_config.yaml、left.yaml、right.yaml三个文件

• realsense_stereo_imu_config.yaml

引用话题如下：

imu_topic: "/camera/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "/home/dji/output/"

外参修改如下：
参考https://blog.csdn.net/mystyle_/article/details/109353870

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1, 0, 0, -0.00552,
           0, 1, 0, 0.0051,
           0, 0, 1, 0.01174,
           0, 0, 0, 1 ]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1, 0, 0, 0.0446571,
           0, 1, 0, 0.0051,
           0, 0, 1, 0.01174,
           0, 0, 0, 1 ]

• left.yaml

left.yaml 对应/camera/infra1/image_rect_raw
因此查看rostopic echo /camera/infra1/camera_info
获得相机内参
得到K: [383.692626953125, 0.0, 318.59832763671875, 0.0, 383.692626953125, 238.12188720703125, 0.0, 0.0, 1.0]
因此left.yaml修改文件如下：

projection_parameters:
   fx: 383.692
   fy: 383.692
   cx: 318.598
   cy: 238.121

• right.yaml

同理right.yaml修改文件如下：

projection_parameters:
   fx: 383.692
   fy: 383.692
   cx: 318.598
   cy: 238.121

3、运行

roslaunch realsense2_camera rs_camera_vins.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
roslaunch vins vins_rviz.launch 

ego部分修改

修改/catkin_ws/src/ego_planner/plan_manage/launch/目录下的single_uav.launch、run_in_xtdrone.launch两个文件
实测后发现ego不需要修改

• single_uav.launch

    <arg name="odom_topic" value="vins_estimator/odometry" />

• run_in_xtdrone.launch

    <!-- camera pose: transform of camera frame in the world frame -->
    <!-- depth topic: depth image, 640x480 by default -->
    <arg name="camera_pose_topic" value="/vins_estimator/camera_pose"/>
    <arg name="depth_topic" value="/camera/depth/image_rect_raw"/>

    <!-- topic of point cloud measurement, such as from LIDAR  -->
    <arg name="cloud_topic" value="/vins_estimator/point_cloud"/>

    <!-- intrinsic params of the depth camera -->
    <arg name="cx" value="383.692"/>
    <arg name="cy" value="383.692"/>
    <arg name="fx" value="318.598"/>
    <arg name="fy" value="238.122"/>

yolo部分更改

修改~/catkin_ws/src/darknet_ros/darknet_ros/launch/目录下的darknet_ros.launch、ros.yaml两个文件

• darknet_ros.launch

  <arg name="image" default="/camera/color/image_raw" />

• ros.yaml

  camera_reading:
    # topic: /camera/rgb/image_raw
    topic: /camera/color/image_raw
    queue_size: 1

注意匹配深度用的话题为/camera/aligned_depth_to_color/image_raw

其他文件修改

1、ego_swarm_transfer.py
不确定是否这样改？

#rospy.Subscriber(vehicle_type+'_'+str(i)+'/mavros/vision_pose/pose', PoseStamped, vision_callback, i,queue_size=1)
#改为
rospy.Subscriber('/vins_estimator/camera_pose', PoseStamped, vision_callback, i,queue_size=1)

2、multi_vins_transfer.py

#rospy.Subscriber(vehicle_type+'_'+str(i)+'/vins_estimator/camera_pose', Odometry, multi_vins_callback, i,queue_size=1)
#rospy.Subscriber(vehicle_type+'_'+str(i)+'/vins_estimator/odometry', Odometry, odom_callback, i,queue_size=1)
#改为
rospy.Subscriber('/vins_estimator/camera_pose', Odometry, multi_vins_callback, i,queue_size=1)
rospy.Subscriber('/vins_estimator/odometry', Odometry, odom_callback, i,queue_size=1)

测试集群后应该把vins加上启动编号

3、yolo_interface.py
接入d435i相机，相应代码全部修改
camera = True部分修改，相应坐标变换部分修改