赞
踩
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三个文件
引用话题如下:
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
对应/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
修改文件如下:
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
修改/catkin_ws/src/ego_planner/plan_manage/launch/目录下的single_uav.launch、run_in_xtdrone.launch两个文件
实测后发现ego不需要修改
<arg name="odom_topic" value="vins_estimator/odometry" />
<!-- 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"/>
修改~/catkin_ws/src/darknet_ros/darknet_ros/launch/目录下的darknet_ros.launch、ros.yaml两个文件
<arg name="image" default="/camera/color/image_raw" />
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部分修改,相应坐标变换部分修改
详见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 #统一发布
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。