当前位置:   article > 正文

D435i+vins-Fusion+ego-planner+yolo无人机避障实测_无人机搭载vins fusi

无人机搭载vins fusi

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"/>
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6

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/"
  • 1
  • 2
  • 3
  • 4

外参修改如下:
参考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 ]
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 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
  • 1
  • 2
  • 3
  • 4
  • 5
  • right.yaml

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

projection_parameters:
   fx: 383.692
   fy: 383.692
   cx: 318.598
   cy: 238.121
  • 1
  • 2
  • 3
  • 4
  • 5

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 
  • 1
  • 2
  • 3

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" />
  • 1
  • 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"/>
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13

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" />
  • 1
  • ros.yaml
  camera_reading:
    # topic: /camera/rgb/image_raw
    topic: /camera/color/image_raw
    queue_size: 1
  • 1
  • 2
  • 3
  • 4

注意匹配深度用的话题为/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)
  • 1
  • 2
  • 3

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)
  • 1
  • 2
  • 3
  • 4
  • 5

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

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

启动文件

详见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   #统一发布
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/2023面试高手/article/detail/250759
推荐阅读
相关标签
  

闽ICP备14008679号