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部分修改，相应坐标变换部分修改

d435i深度滤波

有几次避障测试，发现空地会出现点云，这也是导致避障不能成功的重要原因，添加深度滤波代码

#<arg name="filters"  default=""/>
<arg name="filters" default="spatial, temporal, hole"/>

效果如下

d435i相机标定

当无人机受到撞击时，可能会影响相机内参，因此标定也十分重要，d435i标定参考
https://blog.csdn.net/qq_35616298/article/details/116171823

标定技巧：

1. 保证棋盘格标定板在同一平面，所以可以用ipad，而不是打印的纸张标定板
2. 棋盘格标定板一定要全部包含在相机视野内，否则标定不准确

启动文件

详见multi_plan.sh文件

cd ~/catkin_ws;  source devel/setup.bash; roslaunch darknet_ros task1.launch
cd ~/PX4_Firmware; roslaunch px4 multi_vehicle.launch
cd ~/catkin_ws; roslaunch vins xtdrone_run_vio.launch
cd ~/XTDrone/sensing/slam/vio; python multi_vins_transfer.py iris 3 
cd ~/XTDrone/communication; bash multi_vehicle_communication.sh
cd ~/catkin_ws/src/offboard/scripts; python3 hover.py iris 3 vel
cd ~/XTDrone/motion_planning/3d; python2.7 ego_swarm_transfer.py iris 3
cd ~/catkin_ws; roslaunch ego_planner multi_uav.launch
cd ~/catkin_ws; roslaunch test goal_run.launch
cd ~/catkin_ws; rosrun test yolo_interface.py iris 0
cd ~/catkin_ws/src/test/script; bash ego_swarm_goal_new.sh   #统一发布

最终效果

实测成功避障