????????本文有點(diǎn)長(zhǎng)，可以根據(jù)目錄跳轉(zhuǎn)到想看的部分。因?yàn)榉抡婧蛻?yīng)用環(huán)境不同，可能例程的運(yùn)行方式（輸入話題等）有所不同，但第三部分有關(guān)ORB_SLAM3相機(jī)仿真標(biāo)定、第四部分有關(guān)ORB_SLAM3源碼修改的部分是通用的。
目錄
一、仿真環(huán)境配置
1.雙系統(tǒng)安裝
① 工具準(zhǔn)備
② 啟動(dòng)盤制作
??? i. 格式化U盤
??? ii. 制作啟動(dòng)盤
③ 系統(tǒng)安裝
2. XTDrone仿真系統(tǒng)安裝
二、ORB_SLAM3源碼下載及編譯
1. 源碼下載
2. 編譯
① ORB_SLAM3編譯
② ROS例程編譯
3. 測(cè)試
① realsense2安裝
② realsense_ros安裝
③ 重新編譯ORB_SLAM3
④ 運(yùn)行例程
三、ROS例程的運(yùn)行與修改
1. 相機(jī)標(biāo)定
① 制作標(biāo)定世界
②制作launch文件
③ 運(yùn)行世界并完成標(biāo)定
??? i. 啟動(dòng)Gazebo仿真
??? ii. 啟動(dòng)XTDrone無(wú)人機(jī)控制模塊
??? iii. 啟動(dòng)標(biāo)定節(jié)點(diǎn)完成標(biāo)定
??? iv.標(biāo)定數(shù)據(jù)轉(zhuǎn)換為orb_slam3的相機(jī)標(biāo)定數(shù)據(jù)
2. 修改源碼
3. 運(yùn)行ROS例程
四、修改ORB_SLAM3源碼實(shí)現(xiàn)點(diǎn)云、圖像的ROS消息輸出
1. 源碼分析
① 初始化過(guò)程
② 顯示輸出過(guò)程
③ 應(yīng)用方法
2. 利用pcl與ros實(shí)現(xiàn)點(diǎn)云轉(zhuǎn)存與ROS消息發(fā)布
① 改寫ORB_SLAM3
??? i.修改MapDrawer.cc
????????a.添加PCL頭文件
????????b.修改函數(shù)void MapDrawer::DrawMapPoints()
????????c.修改函數(shù)申明
??? ii.修改Viewer.cc
????????a.添加PCL頭文件，引用外部定義的全局變量
????????b.修改 void Viewer::Run() 函數(shù)
??? iii.修改system.h
????????a.添加PCL頭文件
?????????b.定義一個(gè)結(jié)構(gòu)體變量，包含點(diǎn)云、圖像、追蹤信息
?????????c.申明新定義的函數(shù)
??? iv. 修改system.cc
????????a.添加PCL頭文件，定義全局變量
????????b.修改TrackMonocular函數(shù)，形成新函數(shù)
????????c.修改CMakeList.txt
② 改寫ROS應(yīng)用層
??? i. 修改ros_mono.cc
??? ii. 修改CMakeList.txt
3. 運(yùn)行
五、附錄
附錄一：測(cè)試使用的無(wú)人機(jī)模型文件
附錄二：無(wú)人機(jī)測(cè)試環(huán)境及l(fā)aunch文件
一、仿真環(huán)境配置
????????采用XTDrone實(shí)現(xiàn)無(wú)人機(jī)Gazebo仿真。
1.雙系統(tǒng)安裝
① 工具準(zhǔn)備
- Rufus軟件
????????點(diǎn)擊鏈接下載安裝Rufus軟件，選擇圖示版本。
- Usb3.0接口U盤一個(gè)，推薦容量大于16G
- Ubuntu18.04桌面版鏡像文件
?????????點(diǎn)擊鏈接下載安裝Ubuntu18.04系統(tǒng)鏡像，選擇桌面版鏡像，并設(shè)置好下載路徑確保能找到鏡像。
② 啟動(dòng)盤制作
??? i. 格式化U盤
??? ii. 制作啟動(dòng)盤
????????使用Rufus軟件制作啟動(dòng)盤。選擇對(duì)應(yīng)的U盤，選擇下載好的Ubuntu鏡像文件，其余設(shè)置與圖示相同。等待制作完成關(guān)閉即可。
③ 系統(tǒng)安裝
????????若為雙系統(tǒng)安裝，需壓縮硬盤留出系統(tǒng)安裝空間，單系統(tǒng)安裝可跳過(guò)。需要注意的是，最好預(yù)留出90GB以上的空間，否則如果后期空間不夠很麻煩。
????????將U盤插上電腦、重啟，進(jìn)入boot模式，選擇啟動(dòng)介質(zhì)。（可自行搜索自己電腦進(jìn)入boot的方法）
????????選擇啟動(dòng)盤后，選擇安裝Ubuntu。
????????選擇好語(yǔ)言，點(diǎn)擊下一步（建議選擇English(US)）。
??????? 選擇Normal installation 選項(xiàng)，如下圖所示。
????????選擇Something else 選項(xiàng)，如下圖所示。
????????設(shè)置分區(qū)根據(jù)電腦內(nèi)存和存儲(chǔ)不同而不同，具體參見(jiàn)Ubuntu分區(qū)，勾選后點(diǎn)擊install now。
??????? 最后設(shè)置好用戶名和密碼，等待安裝完成后，重啟。
2. XTDrone仿真系統(tǒng)安裝
??????? 仿真平臺(tái)的所有配置根據(jù)XTDrone文檔進(jìn)行即可。
????????仿真平臺(tái)基礎(chǔ)配置 · 語(yǔ)雀
二、ORB_SLAM3源碼下載及編譯
1. 源碼下載
??????? 原始代碼下載地址：GitHub - UZ-SLAMLab/ORB_SLAM3: ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAMORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM - UZ-SLAMLab/ORB_SLAM3https://github.com/UZ-SLAMLab/ORB_SLAM3??????? 建議使用注釋版本，利于代碼閱讀（第三方提供中文注釋）：GitHub - electech6/ORB_SLAM3_detailed_comments: Detailed comments for ORB-SLAM3Detailed comments for ORB-SLAM3. Contribute to electech6/ORB_SLAM3_detailed_comments development by creating an account on GitHub.https://github.com/electech6/ORB_SLAM3_detailed_comments??????? 因?yàn)橐恍┰騡it clone可能網(wǎng)速很慢，可以下載.zip文件后解壓。
2. 編譯
① ORB_SLAM3編譯
??????? 在編譯之前，需要安裝OpenCV（建議安裝3.2.0版本，這樣與cv_bridge使用的OpenCV版對(duì)應(yīng)，不容易出現(xiàn)版本沖突問(wèn)題）、Eigen3（官方要求高于3.1.0版本）。
??????? OpenCV與Eigen3安裝教程可以自行搜索。其他依賴在項(xiàng)目wiki中有寫明。
??????? 配置完依賴后，執(zhí)行以下代碼進(jìn)行編譯。
cd ORB_SLAM3
chmod +x build.sh
./build.sh??????? 成功編譯完成后，會(huì)在/ORB_SLAM3/lib/目錄下生成libORB_SLAM.so文件。
② ROS例程編譯
????????首先將以下代碼加入~/.bashrc中。
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS???????? 隨后運(yùn)行：
chmod +x build_ros.sh
./build_ros.sh3. 測(cè)試
??????? 可以使用數(shù)據(jù)集測(cè)試，我們這邊用了T265 ROS版本測(cè)試。
① realsense2安裝
??????? 安裝參考：T265驅(qū)動(dòng)安裝_英特爾t265驅(qū)動(dòng)-CSDN博客
??????? 值得注意的是，因?yàn)門265已經(jīng)停產(chǎn)，最新版本的realsense2已經(jīng)不支持T265。建議使用2.50.0版本（librealsense-2.50.0.zip），ROS驅(qū)動(dòng)對(duì)應(yīng)的是2.3.2版本（realsense-ros-2.3.2.zip）
??????? 下載完成后，先配置依賴后編譯realsense2：
# 依賴
sudo apt-get install libudev-dev pkg-config libgtk-3-dev
sudo apt-get install libusb-1.0-0-dev pkg-config
sudo apt-get install libglfw3-dev
sudo apt-get install libssl-dev
./scripts/setup_udev_rules.sh
./scripts/patch-realsense-ubuntu-lts.sh
# 編譯
mkdir build
cd build
cmake ../ -DBUILD_EXAMPLES=true
make
sudo make install ??????? 編譯過(guò)程中，由于網(wǎng)絡(luò)問(wèn)題，curl可能無(wú)法下載安裝?？梢詮膅ithub上下載.zip壓縮包，隨后在CmakeList中修改源即可。參考：realsense git libcurl 失敗解決的方法（親測(cè)）git失敗 改本地壓縮包_realsense2安裝中curl失敗-CSDN博客
② realsense_ros安裝
?????????ROS驅(qū)動(dòng)對(duì)應(yīng)的是2.3.2版本（realsense-ros-2.3.2.zip），編譯過(guò)程如下：
mkdir -p realsense_ros_ws/src
cd realsense_ros_ws/src # 復(fù)制解壓好的realsense-ros-2.3.2到此目錄下
catkin_init_workspace
cd ..
catkin_make clean
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install
echo "source ~/YOUR_WORKSAPCE/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
# 測(cè)試運(yùn)行
roslaunch realsense2_camera rs_t265.launch③ 重新編譯ORB_SLAM3
??????? 因?yàn)镺RB_SLAM3在編譯時(shí)檢測(cè)到未安裝realsense2，則不會(huì)編譯相關(guān)例程。所以，安裝測(cè)試號(hào)realsense2后，重新編譯一次即可。
④ 運(yùn)行例程
# T265 ORBSLAM3
cd <your_dir>/ORB_SLAM3
./Examples/Stereo-InInertial/stereo_inertial_realsense_t265 Vocabulary/ORBvoc.txt ./Examples/Stereo-InInertial/RealSense_T265.yaml
?????????等待初始化完成后 ，彈出兩個(gè)彈窗，一個(gè)是點(diǎn)云顯示與相機(jī)追蹤，還有一個(gè)是雙目相機(jī)的圖像與特征點(diǎn)標(biāo)注。
???????? T265的特征點(diǎn)識(shí)別效果一般，也可能是測(cè)試環(huán)境比較簡(jiǎn)單。
三、ROS例程的運(yùn)行與修改
1. 相機(jī)標(biāo)定
??????? ROS有一個(gè)標(biāo)定模塊camera_calibration，用起來(lái)非常方便。具體可以參考：相機(jī)標(biāo)定原理 用ROS camera_calibration 功能包 在gazebo中進(jìn)行 相機(jī)校準(zhǔn)_gazebo相機(jī)標(biāo)定世界-CSDN博客
① 制作標(biāo)定世界
??????? 首先需要制作Gazebo世界，需要有無(wú)人機(jī)（裝有需要標(biāo)定的相機(jī)）及標(biāo)定板。世界文件calibration.world如下：
<sdf version='1.6'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose frame=''>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<!-- 無(wú)人機(jī) -->
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>300 300</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>300 300</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics name='default_physics' default='0' type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<!-- 起飛標(biāo)志點(diǎn)（非必須） -->
<include>
<uri>model://takeoff</uri>
<pose>-0 -0.5 0 0 1.57 1.57</pose>
</include>
<!-- 標(biāo)定板（注意，此標(biāo)定板為單面的模型，從背面看為透明） -->
<include>
<uri>model://checkerboard_plane</uri>
<pose>5 0 2 0 1.57 3.14</pose>
</include>
<state world_name='default'>
<sim_time>135 449000000</sim_time>
<real_time>136 177712649</real_time>
<wall_time>1656128132 28380767</wall_time>
<iterations>135449</iterations>
<model name='ground_plane'>
<pose frame=''>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose frame=''>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose frame=''>-1.41507 9.30903 47.355 -0 1.4338 -1.39902</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>②制作launch文件
??????? launch文件的制作比較簡(jiǎn)單，可以直接參考XTDrone的其他啟動(dòng)節(jié)點(diǎn)。修改后的calibration.launch文件如下：
<?xml version="1.0"?>
<launch>
<!-- MAVROS posix SITL environment launch script -->
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="world" default="/home/xtdrone/user_ws/modules/slam/Gazebo_Calibration/calibration.world"/>
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
<arg name="world_name" value="$(arg world)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
</include>
<!-- iris_0 -->
<group ns="iris_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="0"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:24540@localhost:34580"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="0"/>
<arg name="z" value="0.5"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_zhihang"/>
<arg name="mavlink_udp_port" value="18570"/>
<arg name="mavlink_tcp_port" value="4560"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->????????其中第七行為上一步calibration.world的存儲(chǔ)地址。
③ 運(yùn)行世界并完成標(biāo)定
??? i. 啟動(dòng)Gazebo仿真
cd ~/user_ws/modules/slam/Gazebo_Calibration # 進(jìn)入到存儲(chǔ)launch文件的目錄下
roslaunch ./calibration.launch??? ii. 啟動(dòng)XTDrone無(wú)人機(jī)控制模塊
# 無(wú)人機(jī)通訊控制節(jié)點(diǎn)
cd ~/XTDrone/communication/
python multirotor_communication.py iris 0# 用鍵盤控制無(wú)人機(jī)飛行
cd ~/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 0 vel
??????? 啟動(dòng)后Gazebo界面如圖所示，鍵盤控制可以控制無(wú)人機(jī)解鎖、起飛等。解鎖后，切換到offboard模式，并起飛無(wú)人機(jī)。
??? iii. 啟動(dòng)標(biāo)定節(jié)點(diǎn)完成標(biāo)定
??????? 運(yùn)行cameracalibrator.py。其中，--size參數(shù)是標(biāo)定板點(diǎn)數(shù)，設(shè)橫向黑白格共n個(gè)，縱向黑白格共m個(gè)，則參數(shù)為(n-1)x(m-1)，這里我們是7x7；--square參數(shù)是標(biāo)定板每個(gè)小格子的邊長(zhǎng)，單位是米，這里模型每個(gè)小格邊長(zhǎng)0.25m；image:=是相機(jī)圖像消息名稱；camera:=是相機(jī)服務(wù)名稱。
rosrun camera_calibration cameracalibrator.py --size 7x7 --square 0.25 image:=/iris_0/realsense/depth_camera/color/image_raw camera:=/iris_0/realsense/depth_camera??????? 啟動(dòng)后如圖：
??????? 此時(shí)，運(yùn)動(dòng)無(wú)人機(jī)使得X,Y,Size,Skew均達(dá)到綠色（橫向、縱向、前后、旋轉(zhuǎn)），calibrate按鈕亮起，即可完成標(biāo)定。
???????? 點(diǎn)擊calibrate按鈕，等待完成計(jì)算，save按鈕會(huì)亮起。
???????? 點(diǎn)擊save按鈕，會(huì)存儲(chǔ)標(biāo)定數(shù)據(jù)到“/tmp/calibrationdata.tar.gz”中。點(diǎn)擊commit退出。
??? iv.標(biāo)定數(shù)據(jù)轉(zhuǎn)換為orb_slam3的相機(jī)標(biāo)定數(shù)據(jù)
??????? 參考：ROS+Opencv的雙目相機(jī)標(biāo)定和orbslam雙目參數(shù)匹配_opencvsharp 雙目相機(jī)-CSDN博客
??????? 標(biāo)定數(shù)據(jù)解壓后找到ost.yaml，我標(biāo)定完數(shù)據(jù)如下：
image_width: 1280
image_height: 720
camera_name: narrow_stereo
camera_matrix:
rows: 3
cols: 3
data: [1109.51051, 0. , 637.897 ,
0. , 1109.55019, 358.77075,
0. , 0. , 1. ]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [0.000544, -0.003422, -0.000323, -0.000315, 0.000000]
rectification_matrix:
rows: 3
cols: 3
data: [1., 0., 0.,
0., 1., 0.,
0., 0., 1.]
projection_matrix:
rows: 3
cols: 4
data: [1109.35156, 0. , 637.53416, 0. ,
0. , 1109.77466, 358.61387, 0. ,
0. , 0. , 1. , 0. ]????????復(fù)制/ORB_SLAM3/Example/ROS/ORB_SLAM3/Asus.yaml文件，并對(duì)其進(jìn)行修改，修改方法如下：
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: "1.0"
Camera.type: "PinHole" # 針孔相機(jī)
# Camera calibration and distortion parameters (OpenCV)
Camera1.fx: 1109.35156 # 對(duì)應(yīng)projection_matrix.data的第1個(gè)參數(shù)
Camera1.fy: 1109.77466 # 對(duì)應(yīng)projection_matrix.data的第6個(gè)參數(shù)
Camera1.cx: 637.53416 # 對(duì)應(yīng)projection_matrix.data的第3個(gè)參數(shù)
Camera1.cy: 358.61387 # 對(duì)應(yīng)projection_matrix.data的第7個(gè)參數(shù)
# 畸變糾正，對(duì)應(yīng)distortion_coefficients矩陣
Camera1.k1: 0.000544
Camera1.k2: -0.003422
Camera1.p1: -0.000323
Camera1.p2: -0.000315
Camera1.k3: 0.000000
# Camera frames per second
Camera.fps: 30
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 0
# Camera resolution
Camera.width: 1280 # 對(duì)應(yīng)image_width
Camera.height: 720 # 對(duì)應(yīng)image_height
#--------------------------------------------------------------------------------------------
# ORB Parameters 以下為ORB的參數(shù)，基本不需要修改
#--------------------------------------------------------------------------------------------
# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000
# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2
# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8
# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7
#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1.0
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500.0
2. 修改源碼
??????? 找到/ORB_SLAM3/Examples/ROS/ORB_SLAM3/src/ros_mono.cc，將第62行中訂閱的消息名稱修改為自己的消息名稱。
ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);??????? 例如XTDrone例程中無(wú)人機(jī)的RGB-D相機(jī)的RGB圖像作為單目輸入源，消息名稱為：“/iris_0/realsense/depth_camera/color/image_raw”：
#define cfg_ROS_IMG_INPUT_TOPIC "/iris_0/realsense/depth_camera/color/image_raw"
ros::Subscriber sub = nodeHandler.subscribe(cfg_ROS_IMG_INPUT_TOPIC, 1, &ImageGrabber::GrabImage, &igb);3. 運(yùn)行ROS例程
??????? 重新編譯后，運(yùn)行（其中depth_camera.yaml是上一步標(biāo)定完成后的標(biāo)定文件）：
# 啟動(dòng)一個(gè)無(wú)人機(jī)飛行場(chǎng)景
roslaunch px4 zhihang2.launch
# 啟動(dòng)控制及通訊節(jié)點(diǎn)
cd ~/user_ws/modules/gui/scripts
python multirotor_communication.py iris 0
cd ~/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 0 vel
# 啟動(dòng)ORB_SLAM3
rosrun ORB_SLAM3 Mono ~/user_ws/modules/slam/ORB_SLAM3/Vocabulary/ORBvoc.txt ~/user_ws/modules/slam/Gazebo_Calibration/data/depth_camera.yaml??????? 運(yùn)行效果如圖：
四、修改ORB_SLAM3源碼實(shí)現(xiàn)點(diǎn)云、圖像的ROS消息輸出
??????? 通過(guò)觀察節(jié)點(diǎn)代碼可以發(fā)現(xiàn)，在ros_mono.cc的倒數(shù)第二行調(diào)用ORB_SLAM3算法：
mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());??????? 此函數(shù)返回值只有相機(jī)追蹤后的估計(jì)位置，并不能輸出點(diǎn)云與圖像。而算法內(nèi)部則已經(jīng)全部封裝到了/ORB_SLAM3/lib/libORB_SLAM3.so中。因此，如果需要將點(diǎn)云及標(biāo)注過(guò)特征點(diǎn)的圖像發(fā)布到ROS消息以供其他處理，則需要修改源碼。
1. 源碼分析
① 初始化過(guò)程
??????? 第一步是初始化ORB_SLAM3系統(tǒng)：
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR,true);??????? 這一步會(huì)調(diào)用/src/system.cc下的初始化函數(shù)：
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
const bool bUseViewer, const int initFr, const string &strSequence):
mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false), mbResetActiveMap(false),
mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), mbShutDown(false)
{ ... }
??????? 在這個(gè)函數(shù)中，會(huì)完成導(dǎo)入數(shù)據(jù)、開(kāi)啟Tracking線程、LocalMapping線程、LoopClosing線程，開(kāi)啟顯示線程。其中，我使用的方法是通過(guò)顯示線程將數(shù)據(jù)導(dǎo)出。
??????? 在該函數(shù)的最后，有創(chuàng)建顯示線程的代碼：
//Initialize the Viewer thread and launch 創(chuàng)建并開(kāi)啟顯示線程
if(bUseViewer)
//if(false) // TODO
{
mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile,settings_);
mptViewer = new thread(&Viewer::Run, mpViewer);
mpTracker->SetViewer(mpViewer);
mpLoopCloser->mpViewer = mpViewer;
mpViewer->both = mpFrameDrawer->both;
}
② 顯示輸出過(guò)程
??????? 顯而易見(jiàn)，算法通過(guò)mptViewer線程運(yùn)行Viewer::Run函數(shù)實(shí)現(xiàn)顯示功能。因此，找到/src/Viewer.cc文件，其中定義了Viewer::Run函數(shù)：
void Viewer::Run() { ... }
??????? 其中，以下這段代碼是處理標(biāo)注過(guò)特征點(diǎn)的圖像。顯然，toShow變量就是存儲(chǔ)了處理完成的圖像。
cv::Mat toShow;
cv::Mat im = mpFrameDrawer->DrawFrame(trackedImageScale);
if(both){
cv::Mat imRight = mpFrameDrawer->DrawRightFrame(trackedImageScale);
cv::hconcat(im,imRight,toShow);
}
else{
toShow = im;
}
if(mImageViewerScale != 1.f) {
int width = toShow.cols * mImageViewerScale;
int height = toShow.rows * mImageViewerScale;
cv::resize(toShow, toShow, cv::Size(width, height));
}
cv::imshow("ORB-SLAM3: Current Frame",toShow);
cv::waitKey(mT);??????? 以下這段代碼是處理點(diǎn)云圖顯示的。然而，我們并不能從這里獲取點(diǎn)云數(shù)據(jù)，因?yàn)樗惴ɡ昧薉rawMapPoints函數(shù)完成了點(diǎn)云的繪制。
d_cam.Activate(s_cam);
glClearColor(1.0f,1.0f,1.0f,1.0f);
mpMapDrawer->DrawCurrentCamera(Twc);
if(menuShowKeyFrames || menuShowGraph || menuShowInertialGraph || menuShowOptLba)
mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph, menuShowInertialGraph, menuShowOptLba);
if(menuShowPoints)
mpMapDrawer->DrawMapPoints(); // 畫出點(diǎn)云??????? 因此，找到/src/MapDrawer.cc文件，找到DrawMapPoints函數(shù)：
void MapDrawer::DrawMapPoints() { ... }??????? 其中，這一部分實(shí)現(xiàn)了點(diǎn)云的繪制：
for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
{
if((*sit)->isBad())
continue;
Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
glVertex3f(pos(0),pos(1),pos(2));
}③ 應(yīng)用方法
??????? 當(dāng)一幀圖像輸入到節(jié)點(diǎn)后，會(huì)調(diào)用TrackMonocular函數(shù)完成視覺(jué)SLAM（以ros_mono.cc為例）：
mpSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec());??????? 由于ORB_SLAM編譯后生成的是lib文件，因此難以在其中嵌入ROS?？紤]到需要將數(shù)據(jù)輸出，可以考慮改寫TrackMonocular函數(shù)，使得函數(shù)能夠返回點(diǎn)云、圖像等數(shù)據(jù)。
2. 利用pcl與ros實(shí)現(xiàn)點(diǎn)云轉(zhuǎn)存與ROS消息發(fā)布
① 改寫ORB_SLAM3
??????? 將點(diǎn)云數(shù)據(jù)及圖像數(shù)據(jù)存儲(chǔ)到一個(gè)全局變量中，然后用修改后的TrackMonocular函數(shù)（TrackMonocularOutput函數(shù)）返回結(jié)構(gòu)體，結(jié)構(gòu)體中包含相機(jī)追蹤數(shù)據(jù)、點(diǎn)云數(shù)據(jù)、圖像數(shù)據(jù)。
??? i.修改MapDrawer.cc
????????a.添加PCL頭文件
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
????????b.修改函數(shù)void MapDrawer::DrawMapPoints()
??????? 修改函數(shù)中以下for循環(huán)：
// 修改前
for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
{
if((*sit)->isBad())
continue;
Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
glVertex3f(pos(0),pos(1),pos(2));
}???????? 在遍歷點(diǎn)云中的點(diǎn)時(shí)，存入pcl點(diǎn)云變量。修改后如下：
// 創(chuàng)建點(diǎn)云圖
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
{
if((*sit)->isBad())
continue;
Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
glVertex3f(pos(0),pos(1),pos(2));
// 將點(diǎn)插入點(diǎn)云圖
pcl::PointXYZ point;
point.x = pos(0);
point.y = pos(1);
point.z = pos(2);
point_cloud->points.push_back(point);
}
// TEST 保存點(diǎn)云圖像到本地
//if(point_cloud->points.size())
// pcl::io::savePCDFileBinary("orb_slam3.pcd", *point_cloud);??????? 這里，可以直接將point_cloud變量存儲(chǔ)到全局變量中。這里我的處理方法是，修改函數(shù)定義，讓其返回 pcl::PointCloud<pcl::PointXYZ>::Ptr 數(shù)據(jù)。修改好的函數(shù)如下：
pcl::PointCloud<pcl::PointXYZ>::Ptr MapDrawer::DrawMapPoints()
{
// 創(chuàng)建點(diǎn)云圖
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
Map* pActiveMap = mpAtlas->GetCurrentMap();
if(!pActiveMap)
return point_cloud;
const vector<MapPoint*> &vpMPs = pActiveMap->GetAllMapPoints();
const vector<MapPoint*> &vpRefMPs = pActiveMap->GetReferenceMapPoints();
set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());
if(vpMPs.empty())
return point_cloud;
glPointSize(mPointSize);
glBegin(GL_POINTS);
glColor3f(0.0,0.0,0.0);
for(size_t i=0, iend=vpMPs.size(); i<iend;i++)
{
if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i]))
continue;
Eigen::Matrix<float,3,1> pos = vpMPs[i]->GetWorldPos();
glVertex3f(pos(0),pos(1),pos(2));
}
glEnd();
glPointSize(mPointSize);
glBegin(GL_POINTS);
glColor3f(1.0,0.0,0.0);
for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
{
if((*sit)->isBad())
continue;
Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
glVertex3f(pos(0),pos(1),pos(2));
// 將點(diǎn)插入點(diǎn)云圖
pcl::PointXYZ point;
point.x = pos(0);
point.y = pos(1);
point.z = pos(2);
point_cloud->points.push_back(point);
}
// 保存點(diǎn)云圖像到本地
//if(point_cloud->points.size())
// pcl::io::savePCDFileBinary("orb_slam3.pcd", *point_cloud);
glEnd();
return point_cloud;
}????????c.修改函數(shù)申明
??????? 在修改函數(shù)返回值后，需要在/include/MapDrawer.h文件中修改函數(shù)申明：
pcl::PointCloud<pcl::PointXYZ>::Ptr DrawMapPoints();??????? （如果直接將變量存入全局變量則無(wú)需這一步操作）
??? ii.修改Viewer.cc
????????a.添加PCL頭文件，引用外部定義的全局變量
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
// 全局變量定義在/src/system.cc
// 本文 <四.2.iv.b> 中介紹
extern pcl::PointCloud<pcl::PointXYZ>::Ptr G_pcToShow;
extern cv::Mat G_imToShow;????????b.修改 void Viewer::Run() 函數(shù)
??????? 原始點(diǎn)云顯示代碼如下，原始的DrawMapPoints函數(shù)沒(méi)有返回值。
if(menuShowPoints)
mpMapDrawer->DrawMapPoints();??????? 將其修改為以下樣式，用全局變量G_pcToShow接收返回的點(diǎn)云數(shù)據(jù)
if(menuShowPoints)
G_pcToShow = mpMapDrawer->DrawMapPoints(); // 處理后的點(diǎn)云顯示
// TEST
//if(G_pcToShow->points.size())
// pcl::io::savePCDFileBinary("orb_slam3.pcd", *G_pcToShow);
??????? 原始圖片顯示代碼如下：
cv::Mat toShow;
cv::Mat im = mpFrameDrawer->DrawFrame(trackedImageScale);
if(both){
cv::Mat imRight = mpFrameDrawer->DrawRightFrame(trackedImageScale);
cv::hconcat(im,imRight,toShow);
}
else{
toShow = im;
}
if(mImageViewerScale != 1.f){
int width = toShow.cols * mImageViewerScale;
int height = toShow.rows * mImageViewerScale;
cv::resize(toShow, toShow, cv::Size(width, height));
}
cv::imshow("ORB-SLAM3: Current Frame",toShow);
cv::waitKey(mT);??????? 修改將其接受圖片的變量換為全局變量G_imToShow，修改后如下：
cv::Mat im = mpFrameDrawer->DrawFrame(trackedImageScale);
if(both){
cv::Mat imRight = mpFrameDrawer->DrawRightFrame(trackedImageScale);
cv::hconcat(im,imRight,G_imToShow);
}
else{
G_imToShow = im;
}
if(mImageViewerScale != 1.f){
int width = G_imToShow.cols * mImageViewerScale;
int height = G_imToShow.rows * mImageViewerScale;
cv::resize(G_imToShow, G_imToShow, cv::Size(width, height));
}
cv::imshow("ORB-SLAM3: Current Frame",G_imToShow); // 處理后的視頻流顯示
cv::waitKey(mT);??? iii.修改system.h
????????a.添加PCL頭文件
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>?????????b.定義一個(gè)結(jié)構(gòu)體變量，包含點(diǎn)云、圖像、追蹤信息
// 利用結(jié)構(gòu)體返回位置、點(diǎn)云、圖像（在 namespace ORB_SLAM3 下）
typedef struct {
pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud;
cv::Mat image;
Sophus::SE3f pos;
}SLAM_Output;
?????????c.申明新定義的函數(shù)
// 返回相機(jī)追蹤信息、點(diǎn)云數(shù)據(jù)、圖像數(shù)據(jù)（位置在原TrackMonocular函數(shù)之后即可）
SLAM_Output TrackMonocularOutput(const cv::Mat &im, const double ×tamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
??? iv. 修改system.cc
????????a.添加PCL頭文件，定義全局變量
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
// 全局變量（用于傳出輸出點(diǎn)云與輸出圖像）
pcl::PointCloud<pcl::PointXYZ>::Ptr G_pcToShow(new pcl::PointCloud<pcl::PointXYZ>());
cv::Mat G_imToShow;????????b.修改TrackMonocular函數(shù)，形成新函數(shù)
SLAM_Output System::TrackMonocularOutput(const cv::Mat &im, const double ×tamp, const vector<IMU::Point>& vImuMeas, string filename)
{
SLAM_Output data;
{
unique_lock<mutex> lock(mMutexReset);
if(mbShutDown)
return data;
}
// 確保是單目或單目VIO模式
if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR)
{
cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular nor Monocular-Inertial." << endl;
exit(-1);
}
cv::Mat imToFeed = im.clone();
if(settings_ && settings_->needToResize()){
cv::Mat resizedIm;
cv::resize(im,resizedIm,settings_->newImSize());
imToFeed = resizedIm;
}
// Check mode change
{
// 獨(dú)占鎖，主要是為了mbActivateLocalizationMode和mbDeactivateLocalizationMode不會(huì)發(fā)生混亂
unique_lock<mutex> lock(mMutexMode);
// mbActivateLocalizationMode為true會(huì)關(guān)閉局部地圖線程，僅跟蹤模式
if(mbActivateLocalizationMode)
{
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{
usleep(1000);
}
// 局部地圖關(guān)閉以后，只進(jìn)行追蹤的線程，只計(jì)算相機(jī)的位姿，沒(méi)有對(duì)局部地圖進(jìn)行更新
mpTracker->InformOnlyTracking(true);
// 關(guān)閉線程可以使得別的線程得到更多的資源
mbActivateLocalizationMode = false;
}
if(mbDeactivateLocalizationMode)
{
mpTracker->InformOnlyTracking(false);
mpLocalMapper->Release();
mbDeactivateLocalizationMode = false;
}
}
// Check reset
{
unique_lock<mutex> lock(mMutexReset);
if(mbReset)
{
mpTracker->Reset();
mbReset = false;
mbResetActiveMap = false;
}
// 如果檢測(cè)到重置活動(dòng)地圖的標(biāo)志為true,將重置地圖
else if(mbResetActiveMap)
{
cout << "SYSTEM -> Reseting active map in monocular case" << endl;
mpTracker->ResetActiveMap();
mbResetActiveMap = false;
}
}
// 如果是單目VIO模式，把IMU數(shù)據(jù)存儲(chǔ)到隊(duì)列mlQueueImuData
if (mSensor == System::IMU_MONOCULAR)
for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
mpTracker->GrabImuData(vImuMeas[i_imu]);
// 計(jì)算相機(jī)位姿
Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename);
// 更新跟蹤狀態(tài)和參數(shù)
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
data.pointcloud = G_pcToShow;
data.image = G_imToShow;
data.pos = Tcw;
return data;
}
????????c.修改CMakeList.txt
??????? 需要再其中添加PCL庫(kù)，在CMakeList中添加如下三行：
...
find_package(PCL REQUIRED) # 添加行
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/include
${PROJECT_SOURCE_DIR}/include/CameraModels
${PROJECT_SOURCE_DIR}/Thirdparty/Sophus
${EIGEN3_INCLUDE_DIR}
${Pangolin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS} # 添加行
)
...
target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PCL_LIBRARIES} # 添加行
${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
-lboost_serialization
-lcrypto
)
????????完成后以上步驟后，可以重新編譯ORB_SLAM3。
② 改寫ROS應(yīng)用層
??? i. 修改ros_mono.cc
??????? ROS節(jié)點(diǎn)改寫后如下：
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<ros/ros.h>
#include<cv_bridge/cv_bridge.h>
#include<opencv2/core/core.hpp>
#include<sensor_msgs/PointCloud2.h>
#include<sensor_msgs/Image.h>
#include<std_msgs/Header.h>
#include"../../../include/System.h"
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
using namespace std;
/***************** <WSJ> CODE ******************/
#define cfg_NODE_NAME "mono"
#define cfg_ROS_IMG_INPUT_TOPIC "/iris_0/realsense/depth_camera/color/image_raw"
#define cfg_ROS_IMG_OUTPUT_TOPIC "/orb_slam3/output/image"
#define cfg_ROS_PC_OUTPUT_TOPIC "/orb_slam3/output/pointcloud2"
#define RED "\e[1;31m"
#define YELLOW "\e[1;33m"
#define GREEN "\e[1;32m"
#define BLUE "\e[1;34m"
#define COLOR_TILE "\e[0m"
ros::Publisher img_pub;
ros::Publisher pc_pub;
sensor_msgs::ImagePtr img_msg;
sensor_msgs::PointCloud2 pc_msg;
/*************** <WSJ> CODE END ****************/
class ImageGrabber
{
public:
ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){}
void GrabImage(const sensor_msgs::ImageConstPtr& msg);
ORB_SLAM3::System* mpSLAM; // 創(chuàng)建SLMA系統(tǒng)指針
};
int main(int argc, char **argv)
{
ros::init(argc, argv, cfg_NODE_NAME);
ros::start();
if(argc != 3)
{
cerr << endl << "Usage: rosrun ORB_SLAM3 Mono path_to_vocabulary path_to_settings" << endl;
ros::shutdown();
return 1;
}
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR,true);
ImageGrabber igb(&SLAM);
ros::NodeHandle nodeHandler;
ros::Subscriber sub = nodeHandler.subscribe(cfg_ROS_IMG_INPUT_TOPIC, 1, &ImageGrabber::GrabImage, &igb);
img_pub = nodeHandler.advertise<sensor_msgs::Image>(cfg_ROS_IMG_OUTPUT_TOPIC, 10);
pc_pub = nodeHandler.advertise<sensor_msgs::PointCloud2>(cfg_ROS_PC_OUTPUT_TOPIC, 10);
cout << GREEN << "\n\n[INFO]<ros_mono.cc>: Node Start." << COLOR_TILE << endl;
cout << "---------------------------------------------------------------------" << endl;
ros::spin();
// Stop all threads
SLAM.Shutdown();
// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
ros::shutdown();
return 0;
}
// ROS接收消息回調(diào)函數(shù)
void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
{
// Copy the ros image message to cv::Mat.
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(msg);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("[ROS]<%s> cv_bridge r2c exception: %s", cfg_NODE_NAME, e.what());
return;
}
//mpSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec()); // SLAM系統(tǒng)追蹤單目相機(jī) 返回相機(jī)位置數(shù)據(jù)
ORB_SLAM3::SLAM_Output data;
data = mpSLAM->TrackMonocularOutput(cv_ptr->image, cv_ptr->header.stamp.toSec());
// 發(fā)布IMG到ROS消息
std_msgs::Header header;
header.frame_id = "camera";
header.stamp = ros::Time::now();
try
{
img_msg = cv_bridge::CvImage(header, "rgb8", data.image).toImageMsg();
img_pub.publish(*img_msg);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("[ROS]<%s> cv_bridge c2r exception: %s", cfg_NODE_NAME, e.what());
return;
}
// 發(fā)布PC到ROS消息
pcl::toROSMsg(*(data.pointcloud), pc_msg);
pc_msg.header = header;
pc_pub.publish(pc_msg);
//cout << data.pos.matrix() << endl << endl;
//if((data.pointcloud)->points.size())
//pcl::io::savePCDFileBinary("orb_slam3.pcd", *(data.pointcloud)); //此行會(huì)莫名其妙導(dǎo)致運(yùn)行<Tracking.cc>623行報(bào)錯(cuò)退出
}???????? 應(yīng)用層中，主要是調(diào)用了上一步已經(jīng)修改好的TrackMonocularOutput函數(shù)替換原來(lái)使用的TrackMonocular函數(shù)，并將返回值通過(guò)ROS發(fā)布。
??? ii. 修改CMakeList.txt
??????? 需要在其中添加PCL庫(kù)，在CMakeList中添加如下2行：
...
find_package(PCL REQUIRED) # 添加行
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/../../../
${PROJECT_SOURCE_DIR}/../../../include
${PROJECT_SOURCE_DIR}/../../../include/CameraModels
${PROJECT_SOURCE_DIR}/../../../Thirdparty/Sophus
${Pangolin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS} # 添加行
)
一些參考資料：
PCL轉(zhuǎn)ROSMSG PointCloud2：ROS與PCL中各種點(diǎn)云數(shù)據(jù)格式之間的轉(zhuǎn)換（大總結(jié)）_pcl::torosmsg-CSDN博客文章瀏覽閱讀2.3k次，點(diǎn)贊11次，收藏63次。ROS與PCL中各種點(diǎn)云數(shù)據(jù)格式之間的轉(zhuǎn)換（大總結(jié)）三種常用點(diǎn)云數(shù)據(jù)格式：pcl::PointCloud< PointT>pcl::PCLPointCloud2snesor_msgs::PointCloud21.sensor_msgs::PointCloud2轉(zhuǎn)pcl::PCLPointCloud2pcl_conversion::toPCl(sensor_msgs::PointCloud2,pcl::PCLPointCloud2)2.sensor_msgs::PointClo_pcl::torosmsghttps://blog.csdn.net/m0_45388819/article/details/113794706OpenCV轉(zhuǎn)ROSMSG Image：利用opencv將本地圖片轉(zhuǎn)換成ROS格式_opencv 圖像類型轉(zhuǎn)換為rosmsg類型-CSDN博客文章瀏覽閱讀884次。轉(zhuǎn)自:http://blog.csdn.net/yake827/article/details/44593621本文主要講解如何將本地的圖片通過(guò)ROS來(lái)顯示出來(lái)。主要利用了OpenCV庫(kù)，一樣是來(lái)源于ROS官網(wǎng).創(chuàng)建一個(gè)ROS工作區(qū)工作區(qū)還是存放和編譯我們的文件[plain] view plain copy$ mkdi_opencv 圖像類型轉(zhuǎn)換為rosmsg類型https://blog.csdn.net/mxgsgtc/article/details/72143054將ORBSLAM中的Map轉(zhuǎn)換為點(diǎn)云：ORB SLAM3 點(diǎn)云地圖保存_orbslam3 點(diǎn)云-CSDN博客文章瀏覽閱讀5.1k次，點(diǎn)贊17次，收藏90次。簡(jiǎn)單修改ORB_SLAM3源碼， 使其能保存pcd格式的點(diǎn)云地圖。_orbslam3 點(diǎn)云https://blog.csdn.net/qq_43591054/article/details/125693886
3. 運(yùn)行
??????? 使用 <三、3> 介紹的步驟運(yùn)行即可，可以在其他節(jié)點(diǎn)接收點(diǎn)云消息和圖像消息。
??????? 正常運(yùn)行后，可以通過(guò)rqt_image_view來(lái)查看圖像數(shù)據(jù)是否正確發(fā)布：
??????? 可以通過(guò) rostopic echo / rostopic hz 來(lái)查看點(diǎn)云數(shù)據(jù)是否正確發(fā)布。如需要通過(guò)rviz來(lái)查看點(diǎn)云數(shù)據(jù)是否正確，則需要新增一個(gè)tf轉(zhuǎn)換（上面代碼中，點(diǎn)云的frame_id為camera）。
??????? 改寫雙目與RGB-D方法與單目相同，因此不再贅述。
五、附錄
附錄一：測(cè)試使用的無(wú)人機(jī)模型文件
iris_all_sensor.sdf
<?xml version="1.0" ?>
<sdf version='1.5'>
<model name='iris_all_sensor'>
<!--iris body-->
<link name='base_link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>1.5</mass>
<inertia>
<ixx>0.029125</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.029125</iyy>
<iyz>0</iyz>
<izz>0.055225</izz>
</inertia>
</inertial>
<collision name='base_link_inertia_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.47 0.47 0.11</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<visual name='base_link_inertia_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://rotors_description/meshes/iris.stl</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/DarkGrey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
</link>
<link name='/imu_link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.015</mass>
<inertia>
<ixx>1e-05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-05</iyy>
<iyz>0</iyz>
<izz>1e-05</izz>
</inertia>
</inertial>
</link>
<joint name='/imu_joint' type='revolute'>
<child>/imu_link</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<effort>0</effort>
<velocity>0</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='rotor_0'>
<pose frame=''>0.13 -0.22 0.023 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000273104</iyy>
<iyz>0</iyz>
<izz>0.000274004</izz>
</inertia>
</inertial>
<collision name='rotor_0_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.128</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode/>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<visual name='rotor_0_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://rotors_description/meshes/iris_prop_ccw.dae</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/Blue</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
</link>
<joint name='rotor_0_joint' type='revolute'>
<child>rotor_0</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='rotor_1'>
<pose frame=''>-0.13 0.2 0.023 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000273104</iyy>
<iyz>0</iyz>
<izz>0.000274004</izz>
</inertia>
</inertial>
<collision name='rotor_1_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.128</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode/>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<visual name='rotor_1_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://rotors_description/meshes/iris_prop_ccw.dae</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/DarkGrey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
</link>
<joint name='rotor_1_joint' type='revolute'>
<child>rotor_1</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='rotor_2'>
<pose frame=''>0.13 0.22 0.023 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000273104</iyy>
<iyz>0</iyz>
<izz>0.000274004</izz>
</inertia>
</inertial>
<collision name='rotor_2_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.128</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode/>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<visual name='rotor_2_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://rotors_description/meshes/iris_prop_cw.dae</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/Blue</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
</link>
<joint name='rotor_2_joint' type='revolute'>
<child>rotor_2</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='rotor_3'>
<pose frame=''>-0.13 -0.2 0.023 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000273104</iyy>
<iyz>0</iyz>
<izz>0.000274004</izz>
</inertia>
</inertial>
<collision name='rotor_3_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.128</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode/>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<visual name='rotor_3_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://rotors_description/meshes/iris_prop_cw.dae</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/DarkGrey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
</link>
<joint name='rotor_3_joint' type='revolute'>
<child>rotor_3</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<plugin name='rosbag' filename='libgazebo_multirotor_base_plugin.so'>
<robotNamespace/>
<linkName>base_link</linkName>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin name='front_right_motor_model' filename='libgazebo_motor_model.so'>
<robotNamespace/>
<jointName>rotor_0_joint</jointName>
<linkName>rotor_0</linkName>
<turningDirection>ccw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1100</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.06</momentConstant>
<commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
<motorNumber>0</motorNumber>
<rotorDragCoefficient>0.000175</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/0</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin name='back_left_motor_model' filename='libgazebo_motor_model.so'>
<robotNamespace/>
<jointName>rotor_1_joint</jointName>
<linkName>rotor_1</linkName>
<turningDirection>ccw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1100</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.06</momentConstant>
<commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
<motorNumber>1</motorNumber>
<rotorDragCoefficient>0.000175</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/1</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin name='front_left_motor_model' filename='libgazebo_motor_model.so'>
<robotNamespace/>
<jointName>rotor_2_joint</jointName>
<linkName>rotor_2</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1100</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.06</momentConstant>
<commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
<motorNumber>2</motorNumber>
<rotorDragCoefficient>0.000175</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/2</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin name='back_right_motor_model' filename='libgazebo_motor_model.so'>
<robotNamespace/>
<jointName>rotor_3_joint</jointName>
<linkName>rotor_3</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1100</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.06</momentConstant>
<commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
<motorNumber>3</motorNumber>
<rotorDragCoefficient>0.000175</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/3</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin name='gps_plugin' filename='libgazebo_gps_plugin.so'>
<robotNamespace/>
<gpsNoise>1</gpsNoise>
</plugin>
<plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
<robotNamespace/>
<pubRate>100</pubRate>
<noiseDensity>0.0004</noiseDensity>
<randomWalk>6.4e-06</randomWalk>
<biasCorrelationTime>600</biasCorrelationTime>
<magTopic>/mag</magTopic>
</plugin>
<plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
<robotNamespace/>
<pubRate>50</pubRate>
<baroTopic>/baro</baroTopic>
</plugin>
<plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
<robotNamespace/>
<imuSubTopic>/imu</imuSubTopic>
<gpsSubTopic>/gps</gpsSubTopic>
<magSubTopic>/mag</magSubTopic>
<baroSubTopic>/baro</baroSubTopic>
<mavlink_addr>INADDR_ANY</mavlink_addr>
<mavlink_udp_port>14560</mavlink_udp_port>
<mavlink_tcp_port>4560</mavlink_tcp_port>
<serialEnabled>0</serialEnabled>
<serialDevice>/dev/ttyACM0</serialDevice>
<baudRate>921600</baudRate>
<qgc_addr>INADDR_ANY</qgc_addr>
<qgc_udp_port>14550</qgc_udp_port>
<sdk_addr>INADDR_ANY</sdk_addr>
<sdk_udp_port>14540</sdk_udp_port>
<hil_mode>0</hil_mode>
<hil_state_level>0</hil_state_level>
<vehicle_is_tailsitter>0</vehicle_is_tailsitter>
<send_vision_estimation>1</send_vision_estimation>
<send_odometry>0</send_odometry>
<enable_lockstep>1</enable_lockstep>
<use_tcp>1</use_tcp>
<motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
<control_channels>
<channel name='rotor1'>
<input_index>0</input_index>
<input_offset>0</input_offset>
<input_scaling>1000</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>100</zero_position_armed>
<joint_control_type>velocity</joint_control_type>
</channel>
<channel name='rotor2'>
<input_index>1</input_index>
<input_offset>0</input_offset>
<input_scaling>1000</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>100</zero_position_armed>
<joint_control_type>velocity</joint_control_type>
</channel>
<channel name='rotor3'>
<input_index>2</input_index>
<input_offset>0</input_offset>
<input_scaling>1000</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>100</zero_position_armed>
<joint_control_type>velocity</joint_control_type>
</channel>
<channel name='rotor4'>
<input_index>3</input_index>
<input_offset>0</input_offset>
<input_scaling>1000</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>100</zero_position_armed>
<joint_control_type>velocity</joint_control_type>
</channel>
<channel name='rotor5'>
<input_index>4</input_index>
<input_offset>1</input_offset>
<input_scaling>324.6</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>0</zero_position_armed>
<joint_control_type>velocity</joint_control_type>
<joint_control_pid>
<p>0.1</p>
<i>0</i>
<d>0</d>
<iMax>0.0</iMax>
<iMin>0.0</iMin>
<cmdMax>2</cmdMax>
<cmdMin>-2</cmdMin>
</joint_control_pid>
<joint_name>zephyr_delta_wing::propeller_joint</joint_name>
</channel>
<channel name='rotor6'>
<input_index>5</input_index>
<input_offset>0</input_offset>
<input_scaling>0.524</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>0</zero_position_armed>
<joint_control_type>position</joint_control_type>
<joint_name>zephyr_delta_wing::flap_left_joint</joint_name>
<joint_control_pid>
<p>10.0</p>
<i>0</i>
<d>0</d>
<iMax>0</iMax>
<iMin>0</iMin>
<cmdMax>20</cmdMax>
<cmdMin>-20</cmdMin>
</joint_control_pid>
</channel>
<channel name='rotor7'>
<input_index>6</input_index>
<input_offset>0</input_offset>
<input_scaling>0.524</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>0</zero_position_armed>
<joint_control_type>position</joint_control_type>
<joint_name>zephyr_delta_wing::flap_right_joint</joint_name>
<joint_control_pid>
<p>10.0</p>
<i>0</i>
<d>0</d>
<iMax>0</iMax>
<iMin>0</iMin>
<cmdMax>20</cmdMax>
<cmdMin>-20</cmdMin>
</joint_control_pid>
</channel>
<channel name='rotor8'>
<input_index>7</input_index>
<input_offset>0</input_offset>
<input_scaling>0.524</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>0</zero_position_armed>
<joint_control_type>position</joint_control_type>
</channel>
</control_channels>
</plugin>
<static>0</static>
<plugin name='rotors_gazebo_imu_plugin' filename='libgazebo_imu_plugin.so'>
<robotNamespace/>
<linkName>/imu_link</linkName>
<imuTopic>/imu</imuTopic>
<gyroscopeNoiseDensity>0.00018665</gyroscopeNoiseDensity>
<gyroscopeRandomWalk>3.8785e-05</gyroscopeRandomWalk>
<gyroscopeBiasCorrelationTime>1000.0</gyroscopeBiasCorrelationTime>
<gyroscopeTurnOnBiasSigma>0.0087</gyroscopeTurnOnBiasSigma>
<accelerometerNoiseDensity>0.00186</accelerometerNoiseDensity>
<accelerometerRandomWalk>0.006</accelerometerRandomWalk>
<accelerometerBiasCorrelationTime>300.0</accelerometerBiasCorrelationTime>
<accelerometerTurnOnBiasSigma>0.196</accelerometerTurnOnBiasSigma>
</plugin>
<include>
<uri>model://realsense_camera</uri>
<pose>0.1 0 0 0 0 0</pose>
</include>
<joint name="realsense_camera_joint" type="revolute">
<child>realsense_camera::link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<include>
<uri>model://monocular_camera</uri>
<pose>0 0 -0.03 0 1.5707963 0</pose>
</include>
<joint name="monocular_down_joint" type="fixed">
<child>monocular_camera::link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<!-- For Hokuyo Lidar Payload -->
<include>
<uri>model://hokuyo_lidar</uri>
<pose>0 0 0.06 0 0 0</pose>
</include>
<joint name="lidar_joint" type="fixed">
<child>hokuyo_lidar::link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<!-- For laser Range Finder Payload -->
<include>
<uri>model://laser_rangefinder</uri>
<pose>0 0 -0.05 0 1.570796 0</pose>
</include>
<joint name="laser_1d_joint" type="fixed">
<child>laser_rangefinder::link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<!-- For IMU-->
<include>
<uri>model://imu_gazebo</uri>
<pose>0 0 0.3 0 0 0</pose>
</include>
<joint name="imu_gazebo_joint" type="fixed">
<child>imu_gazebo::link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
</model>
</sdf>
<!-- vim: set noet fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
model.config
<?xml version="1.0"?>
<model>
<name>Iris with all sensor</name>
<version>1.0</version>
<sdf version='1.5'>iris_all_sensor.sdf</sdf>
<author>
<name>wsj</name>
<email>wangshujie@nuaa.edu.cn</email>
</author>
<description>
This is a model of the 3DR Iris Quadrotor for Zhihangcup competition.
</description>
</model>
附錄二：無(wú)人機(jī)測(cè)試環(huán)境及l(fā)aunch文件
map_maze_target.world文章來(lái)源：http://www.zghlxwxcb.cn/news/detail-846798.html
<sdf version='1.6'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose frame=''>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name='ceiling'>
<link name='link'>
<inertial>
<pose frame=''>0 0 0 0 0 0</pose>
<mass>100000</mass>
<inertia>
<ixx>100000</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>100000</iyy>
<iyz>0</iyz>
<izz>100000</izz>
</inertia>
</inertial>
<collision name='collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>300 300 0.5</size>
</box>
</geometry>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode>
<kp>1e+07</kp>
<kd>1</kd>
<min_depth>0.001</min_depth>
<max_vel>0.1</max_vel>
</ode>
</contact>
<bounce/>
<max_contacts>10</max_contacts>
</collision>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose frame=''>0 0 5 0 0 0</pose>
</model>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>300 300</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>300 300</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics name='default_physics' default='0' type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='map2_1'>
<pose frame=''>1.002 -6.58047 0 0 -0 0</pose>
<link name='Wall_0'>
<collision name='Wall_0_Collision'>
<geometry>
<box>
<size>7.75 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_0_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>7.75 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-18.6196 5.51467 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_1'>
<collision name='Wall_1_Collision'>
<geometry>
<box>
<size>37.3896 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_1_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>37.3896 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>0.0002 9.31467 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_10'>
<collision name='Wall_10_Collision'>
<geometry>
<box>
<size>3.93638 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_10_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.93638 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-13.0365 1.84322 0 0 -0 -0.012287</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_11'>
<collision name='Wall_11_Collision'>
<geometry>
<box>
<size>3.84334 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_11_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.84334 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-11.1667 -0.073086 0 0 -0 -1.58339</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_12'>
<collision name='Wall_12_Collision'>
<geometry>
<box>
<size>11.25 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_12_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>11.25 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-5.63996 -1.96613 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_14'>
<collision name='Wall_14_Collision'>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_14_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-0.08996 -0.041134 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_15'>
<collision name='Wall_15_Collision'>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_15_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>1.83504 1.88387 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_17'>
<collision name='Wall_17_Collision'>
<geometry>
<box>
<size>3.75 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_17_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.75 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-7.48571 -3.74848 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_19'>
<collision name='Wall_19_Collision'>
<geometry>
<box>
<size>3.84422 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_19_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.84422 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>5.64847 5.54188 0 0 -0 -3.1164</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_2'>
<collision name='Wall_2_Collision'>
<geometry>
<box>
<size>18.7327 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_2_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>18.7327 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>18.5502 -0.023168 0 0 -0 -1.5733</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_20'>
<collision name='Wall_20_Collision'>
<geometry>
<box>
<size>7.5 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_20_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>7.5 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>3.75542 1.77383 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_21'>
<collision name='Wall_21_Collision'>
<geometry>
<box>
<size>3.75 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_21_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.75 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>5.60195 -1.85465 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_23'>
<collision name='Wall_23_Collision'>
<geometry>
<box>
<size>7.5 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_23_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>7.5 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>7.40195 -5.52965 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_25'>
<collision name='Wall_25_Collision'>
<geometry>
<box>
<size>3.75 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_25_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.75 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-3.76379 -7.45273 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_26'>
<collision name='Wall_26_Collision'>
<geometry>
<box>
<size>7.75 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_26_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>7.75 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>0.036208 -5.65273 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_28'>
<collision name='Wall_28_Collision'>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_28_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>11.1704 -7.37425 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_30'>
<collision name='Wall_30_Collision'>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_30_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>9.28042 1.81996 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_32'>
<collision name='Wall_32_Collision'>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_32_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>13.0603 5.5884 0 0 -0 3.14159</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_33'>
<collision name='Wall_33_Collision'>
<geometry>
<box>
<size>7.70353 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_33_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>7.70353 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>11.1499 1.81166 0 0 -0 -1.56694</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_34'>
<collision name='Wall_34_Collision'>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_34_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>13.0895 -1.96508 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_35'>
<collision name='Wall_35_Collision'>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_35_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>14.968 -0.040077 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_36'>
<collision name='Wall_36_Collision'>
<geometry>
<box>
<size>3.72116 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_36_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.72116 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>16.7999 1.86166 0 0 -0 -0.013028</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_38'>
<collision name='Wall_38_Collision'>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_38_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-11.1611 7.38531 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_39'>
<collision name='Wall_39_Collision'>
<geometry>
<box>
<size>3.84305 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_39_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.84305 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-9.31457 5.46031 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_4'>
<collision name='Wall_4_Collision'>
<geometry>
<box>
<size>37.3438 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_4_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>37.3438 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-0.023062 -9.27664 0 0 -0 -3.14113</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_40'>
<collision name='Wall_40_Collision'>
<geometry>
<box>
<size>3.75 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_40_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.75 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-7.51457 3.66031 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_42'>
<collision name='Wall_42_Collision'>
<geometry>
<box>
<size>7.75 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_42_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>7.75 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-3.76379 5.46379 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_44'>
<collision name='Wall_44_Collision'>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_44_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>4 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-1.83879 5.54188 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_5'>
<collision name='Wall_5_Collision'>
<geometry>
<box>
<size>7.5 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_5_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>7.5 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-18.6196 -5.61033 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_7'>
<collision name='Wall_7_Collision'>
<geometry>
<box>
<size>11.5 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_7_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>11.5 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-14.9546 -0.004511 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_8'>
<collision name='Wall_8_Collision'>
<geometry>
<box>
<size>3.9604 0.2 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_8_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.9604 0.2 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Bricks</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose frame=''>-13.0498 -5.63906 0 0 -0 0.021236</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<static>1</static>
</model>
<state world_name='default'>
<sim_time>493 628000000</sim_time>
<real_time>421 177324053</real_time>
<wall_time>1659794100 482234418</wall_time>
<iterations>420094</iterations>
<model name='map2_1'>
<pose frame=''>1.79516 0.071786 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='Wall_0'>
<pose frame=''>-16.8244 5.58646 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_1'>
<pose frame=''>1.79536 9.38646 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_10'>
<pose frame=''>-11.2413 1.91501 0 0 0 -0.012287</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_11'>
<pose frame=''>-9.37154 -0.001304 0 0 0 -1.58339</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_12'>
<pose frame=''>-3.8448 -1.89434 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_14'>
<pose frame=''>1.7052 0.030656 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_15'>
<pose frame=''>3.6302 1.95566 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_17'>
<pose frame=''>-5.69055 -3.67674 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_19'>
<pose frame=''>7.44363 5.61367 0 0 0 -3.1164</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_2'>
<pose frame=''>20.3454 0.048616 0 0 0 -1.5733</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_20'>
<pose frame=''>5.55058 1.84562 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_21'>
<pose frame=''>7.39711 -1.78286 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_23'>
<pose frame=''>9.19711 -5.45784 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_25'>
<pose frame=''>-1.96863 -7.38094 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_26'>
<pose frame=''>1.83137 -5.58094 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_28'>
<pose frame=''>12.9656 -7.30244 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_30'>
<pose frame=''>11.0756 1.89175 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_32'>
<pose frame=''>14.8555 5.66019 0 0 -0 3.14159</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_33'>
<pose frame=''>12.9451 1.88345 0 0 0 -1.56694</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_34'>
<pose frame=''>14.8847 -1.89329 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_35'>
<pose frame=''>16.7632 0.031706 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_36'>
<pose frame=''>18.5951 1.93345 0 0 0 -0.013028</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_38'>
<pose frame=''>-9.36594 7.4571 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_39'>
<pose frame=''>-7.51941 5.5321 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_4'>
<pose frame=''>1.7721 -9.20484 0 0 0 -3.14113</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_40'>
<pose frame=''>-5.71941 3.7321 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_42'>
<pose frame=''>-1.96863 5.53558 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_44'>
<pose frame=''>-0.043632 5.61367 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_5'>
<pose frame=''>-16.8244 -5.53854 0 0 -0 1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_7'>
<pose frame=''>-13.1594 0.067276 0 0 0 -1.5708</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
<link name='Wall_8'>
<pose frame=''>-11.2546 -5.56724 0 0 -0 0.021236</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='ground_plane'>
<pose frame=''>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='landing2'>
<pose frame=''>18.3012 -0.3 0 0 1.57 1.57</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>18.3012 -0.3 0 0 1.57 1.57</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='takeoff'>
<pose frame=''>-16 -0.5 0 0 1.57 1.57</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>-16 -0.5 0 0 1.57 1.57</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='target_red'>
<pose frame=''>-8.97289 -3.7558 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>-8.97289 -3.7558 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='target_blue'>
<pose frame=''>10.9445 8.81946 0.18348 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>10.9445 8.81946 0.18348 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='target_green'>
<pose frame=''>18.5856 -8.05752 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>18.5856 -8.05752 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose frame=''>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose frame=''>16.8887 -5.88816 80.8158 0 1.398 2.67275</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
<model name='landing2'>
<static>1</static>
<link name='link'>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://landing2/meshes/landing2.dae</uri>
<scale>2 4 4</scale>
</mesh>
</geometry>
</visual>
<collision name='Landing_2_Collision'>
<geometry>
<mesh>
<uri>model://landing2/meshes/landing2.dae</uri>
<scale>2 4 4</scale>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='Landing_2_Collision'>
<geometry>
<mesh>
<uri>model://landing2/meshes/landing2.dae</uri>
<scale>2 4 4</scale>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose frame=''>17 0 0 0 1.57 1.57</pose>
</model>
<model name='takeoff'>
<static>1</static>
<link name='link'>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://takeoff/meshes/takeoff.dae</uri>
<scale>2 2 2</scale>
</mesh>
</geometry>
</visual>
<collision name='Takeoff_Collision'>
<geometry>
<mesh>
<uri>model://takeoff/meshes/takeoff.dae</uri>
<scale>2 2 2</scale>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose frame=''>-18 -0.5 0 0 1.57 1.57</pose>
</model>
<model name='target_red'>
<static>0</static>
<link name='link'>
<pose frame=''>0 0 0 0 0 0</pose>
<collision name='Target_Red_Collision'>
<geometry>
<mesh>
<uri>model://target_red/meshes/target_red.dae</uri>
<scale>0.9 0.9 0.9</scale>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
<pose frame=''>0 0 -0.225 0 0 3.1415</pose>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://target_red/meshes/target_red.dae</uri>
<scale>0.9 0.9 0.9</scale>
</mesh>
</geometry>
<pose frame=''>0 0 -0.225 0 0 3.1415</pose>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
<gravity>0</gravity>
</link>
<pose frame=''>-8.97289 -3.7558 0 0 -0 0</pose>
</model>
<model name='target_blue'>
<static>0</static>
<link name='link'>
<pose frame=''>0 0 0 0 0 0</pose>
<collision name='Target_Blue_Collision'>
<geometry>
<mesh>
<uri>model://target_blue/meshes/target_blue.dae</uri>
<scale>0.9 0.9 0.9</scale>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
<pose frame=''>0 0 -0.225 0 0 3.1415</pose>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://target_blue/meshes/target_blue.dae</uri>
<scale>0.9 0.9 0.9</scale>
</mesh>
</geometry>
<pose frame=''>0 0 -0.225 0 0 3.1415</pose>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
<gravity>0</gravity>
</link>
<pose frame=''>11.0983 1.98454 0 0 -0 0</pose>
</model>
<model name='target_green'>
<static>0</static>
<link name='link'>
<pose frame=''>0 0 0 0 0 0</pose>
<collision name='Target_Green_Collision'>
<geometry>
<mesh>
<uri>model://target_green/meshes/target_green.dae</uri>
<scale>0.9 0.9 0.9</scale>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
<pose frame=''>0 0 -0.225 0 0 3.1415</pose>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://target_green/meshes/target_green.dae</uri>
<scale>0.9 0.9 0.9</scale>
</mesh>
</geometry>
<pose frame=''>0 0 -0.225 0 0 3.1415</pose>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
<gravity>0</gravity>
</link>
<pose frame=''>15.6869 -7.2778 0 0 -0 0</pose>
</model>
</world>
</sdf>map_maze_target.launch文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-846798.html
<?xml version="1.0"?>
<launch>
<!-- MAVROS posix SITL environment launch script -->
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="world" default="/home/xtdrone/user_ws/run/uav_env/map_maze_target.world"/>
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
<arg name="world_name" value="$(arg world)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
</include>
<!-- iris_0 -->
<group ns="iris_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="0"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:24540@localhost:34580"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="-16"/> <!-- -16 for map2-1, -18 for map2-2, -18 for map2-3 -->
<arg name="y" value="0"/>
<arg name="z" value="0.5"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_all_sensor"/>
<arg name="mavlink_udp_port" value="18570"/>
<arg name="mavlink_tcp_port" value="4560"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->到了這里，關(guān)于ORB_SLAM3配置及修改——將圖像、點(diǎn)云用ROS消息發(fā)布（基于無(wú)人機(jī)仿真）的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
