本文有点长，可以根据目录跳转到想看的部分。因为仿真和应用环境不同，可能例程的运行方式（输入话题等）有所不同，但第三部分有关ORB_SLAM3相机仿真标定、第四部分有关ORB_SLAM3源码修改的部分是通用的。
目录
一、仿真环境配置
1.双系统安装
① 工具准备
② 启动盘制作
i. 格式化U盘
ii. 制作启动盘
③ 系统安装
2. XTDrone仿真系统安装
二、ORB_SLAM3源码下载及编译
1. 源码下载
2. 编译
① ORB_SLAM3编译
② ROS例程编译
3. 测试
① realsense2安装
② realsense_ros安装
③ 重新编译ORB_SLAM3
④ 运行例程
三、ROS例程的运行与修改
1. 相机标定
① 制作标定世界
②制作launch文件
③ 运行世界并完成标定
i. 启动Gazebo仿真
ii. 启动XTDrone无人机控制模块
iii. 启动标定节点完成标定
iv.标定数据转换为orb_slam3的相机标定数据
2. 修改源码
3. 运行ROS例程
四、修改ORB_SLAM3源码实现点云、图像的ROS消息输出
1. 源码分析
① 初始化过程
② 显示输出过程
③ 应用方法
2. 利用pcl与ros实现点云转存与ROS消息发布
① 改写ORB_SLAM3
i.修改MapDrawer.cc
a.添加PCL头文件
b.修改函数void MapDrawer::DrawMapPoints()
c.修改函数申明
ii.修改Viewer.cc
a.添加PCL头文件，引用外部定义的全局变量
b.修改 void Viewer::Run() 函数
iii.修改system.h
a.添加PCL头文件
b.定义一个结构体变量，包含点云、图像、追踪信息
c.申明新定义的函数
iv. 修改system.cc
a.添加PCL头文件，定义全局变量
b.修改TrackMonocular函数，形成新函数
c.修改CMakeList.txt
② 改写ROS应用层
i. 修改ros_mono.cc
ii. 修改CMakeList.txt
3. 运行
五、附录
附录一：测试使用的无人机模型文件
附录二：无人机测试环境及launch文件
一、仿真环境配置
采用XTDrone实现无人机Gazebo仿真。
1.双系统安装
① 工具准备
- Rufus软件
点击链接下载安装Rufus软件，选择图示版本。
- Usb3.0接口U盘一个，推荐容量大于16G
- Ubuntu18.04桌面版镜像文件
点击链接下载安装Ubuntu18.04系统镜像，选择桌面版镜像，并设置好下载路径确保能找到镜像。
② 启动盘制作
i. 格式化U盘
ii. 制作启动盘
使用Rufus软件制作启动盘。选择对应的U盘，选择下载好的Ubuntu镜像文件，其余设置与图示相同。等待制作完成关闭即可。
③ 系统安装
若为双系统安装，需压缩硬盘留出系统安装空间，单系统安装可跳过。需要注意的是，最好预留出90GB以上的空间，否则如果后期空间不够很麻烦。
将U盘插上电脑、重启，进入boot模式，选择启动介质。（可自行搜索自己电脑进入boot的方法）
选择启动盘后，选择安装Ubuntu。
选择好语言，点击下一步（建议选择English(US)）。
选择Normal installation 选项，如下图所示。
选择Something else 选项，如下图所示。
设置分区根据电脑内存和存储不同而不同，具体参见Ubuntu分区，勾选后点击install now。
最后设置好用户名和密码，等待安装完成后，重启。
2. XTDrone仿真系统安装
仿真平台的所有配置根据XTDrone文档进行即可。
仿真平台基础配置 · 语雀
二、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 因为一些原因git clone可能网速很慢，可以下载.zip文件后解压。
2. 编译
① ORB_SLAM3编译
在编译之前，需要安装OpenCV（建议安装3.2.0版本，这样与cv_bridge使用的OpenCV版对应，不容易出现版本冲突问题）、Eigen3（官方要求高于3.1.0版本）。
OpenCV与Eigen3安装教程可以自行搜索。其他依赖在项目wiki中有写明。
配置完依赖后，执行以下代码进行编译。
cd ORB_SLAM3
chmod +x build.sh
./build.sh成功编译完成后，会在/ORB_SLAM3/lib/目录下生成libORB_SLAM.so文件。
② ROS例程编译
首先将以下代码加入~/.bashrc中。
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS随后运行：
chmod +x build_ros.sh
./build_ros.sh3. 测试
可以使用数据集测试，我们这边用了T265 ROS版本测试。
① realsense2安装
安装参考：T265驱动安装_英特尔t265驱动-CSDN博客
值得注意的是，因为T265已经停产，最新版本的realsense2已经不支持T265。建议使用2.50.0版本（librealsense-2.50.0.zip），ROS驱动对应的是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 编译过程中，由于网络问题，curl可能无法下载安装。可以从github上下载.zip压缩包，随后在CmakeList中修改源即可。参考：realsense git libcurl 失败解决的方法（亲测）git失败 改本地压缩包_realsense2安装中curl失败-CSDN博客
② realsense_ros安装
ROS驱动对应的是2.3.2版本（realsense-ros-2.3.2.zip），编译过程如下：
mkdir -p realsense_ros_ws/src
cd realsense_ros_ws/src # 复制解压好的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
# 测试运行
roslaunch realsense2_camera rs_t265.launch③ 重新编译ORB_SLAM3
因为ORB_SLAM3在编译时检测到未安装realsense2，则不会编译相关例程。所以，安装测试号realsense2后，重新编译一次即可。
④ 运行例程
# T265 ORBSLAM3
cd <your_dir>/ORB_SLAM3
./Examples/Stereo-InInertial/stereo_inertial_realsense_t265 Vocabulary/ORBvoc.txt ./Examples/Stereo-InInertial/RealSense_T265.yaml
等待初始化完成后 ，弹出两个弹窗，一个是点云显示与相机追踪，还有一个是双目相机的图像与特征点标注。
T265的特征点识别效果一般，也可能是测试环境比较简单。
三、ROS例程的运行与修改
1. 相机标定
ROS有一个标定模块camera_calibration，用起来非常方便。具体可以参考：相机标定原理 用ROS camera_calibration 功能包 在gazebo中进行 相机校准_gazebo相机标定世界-CSDN博客
① 制作标定世界
首先需要制作Gazebo世界，需要有无人机（装有需要标定的相机）及标定板。世界文件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>
<!-- 无人机 -->
<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>
<!-- 起飞标志点（非必须） -->
<include>
<uri>model://takeoff</uri>
<pose>-0 -0.5 0 0 1.57 1.57</pose>
</include>
<!-- 标定板（注意，此标定板为单面的模型，从背面看为透明） -->
<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文件的制作比较简单，可以直接参考XTDrone的其他启动节点。修改后的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的存储地址。
③ 运行世界并完成标定
i. 启动Gazebo仿真
cd ~/user_ws/modules/slam/Gazebo_Calibration # 进入到存储launch文件的目录下
roslaunch ./calibration.launchii. 启动XTDrone无人机控制模块
# 无人机通讯控制节点
cd ~/XTDrone/communication/
python multirotor_communication.py iris 0# 用键盘控制无人机飞行
cd ~/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 0 vel
启动后Gazebo界面如图所示，键盘控制可以控制无人机解锁、起飞等。解锁后，切换到offboard模式，并起飞无人机。
iii. 启动标定节点完成标定
运行cameracalibrator.py。其中，--size参数是标定板点数，设横向黑白格共n个，纵向黑白格共m个，则参数为(n-1)x(m-1)，这里我们是7x7；--square参数是标定板每个小格子的边长，单位是米，这里模型每个小格边长0.25m；image:=是相机图像消息名称；camera:=是相机服务名称。
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启动后如图：
此时，运动无人机使得X,Y,Size,Skew均达到绿色（横向、纵向、前后、旋转），calibrate按钮亮起，即可完成标定。
点击calibrate按钮，等待完成计算，save按钮会亮起。
点击save按钮，会存储标定数据到“/tmp/calibrationdata.tar.gz”中。点击commit退出。
iv.标定数据转换为orb_slam3的相机标定数据
参考：ROS+Opencv的双目相机标定和orbslam双目参数匹配_opencvsharp 双目相机-CSDN博客
标定数据解压后找到ost.yaml，我标定完数据如下：
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. ]复制/ORB_SLAM3/Example/ROS/ORB_SLAM3/Asus.yaml文件，并对其进行修改，修改方法如下：
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: "1.0"
Camera.type: "PinHole" # 针孔相机
# Camera calibration and distortion parameters (OpenCV)
Camera1.fx: 1109.35156 # 对应projection_matrix.data的第1个参数
Camera1.fy: 1109.77466 # 对应projection_matrix.data的第6个参数
Camera1.cx: 637.53416 # 对应projection_matrix.data的第3个参数
Camera1.cy: 358.61387 # 对应projection_matrix.data的第7个参数
# 畸变纠正，对应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 # 对应image_width
Camera.height: 720 # 对应image_height
#--------------------------------------------------------------------------------------------
# ORB Parameters 以下为ORB的参数，基本不需要修改
#--------------------------------------------------------------------------------------------
# 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例程中无人机的RGB-D相机的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. 运行ROS例程
重新编译后，运行（其中depth_camera.yaml是上一步标定完成后的标定文件）：
# 启动一个无人机飞行场景
roslaunch px4 zhihang2.launch
# 启动控制及通讯节点
cd ~/user_ws/modules/gui/scripts
python multirotor_communication.py iris 0
cd ~/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 0 vel
# 启动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运行效果如图：
四、修改ORB_SLAM3源码实现点云、图像的ROS消息输出
通过观察节点代码可以发现，在ros_mono.cc的倒数第二行调用ORB_SLAM3算法：
mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());此函数返回值只有相机追踪后的估计位置，并不能输出点云与图像。而算法内部则已经全部封装到了/ORB_SLAM3/lib/libORB_SLAM3.so中。因此，如果需要将点云及标注过特征点的图像发布到ROS消息以供其他处理，则需要修改源码。
1. 源码分析
① 初始化过程
第一步是初始化ORB_SLAM3系统：
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR,true);这一步会调用/src/system.cc下的初始化函数：
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)
{ ... }
在这个函数中，会完成导入数据、开启Tracking线程、LocalMapping线程、LoopClosing线程，开启显示线程。其中，我使用的方法是通过显示线程将数据导出。
在该函数的最后，有创建显示线程的代码：
//Initialize the Viewer thread and launch 创建并开启显示线程
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;
}
② 显示输出过程
显而易见，算法通过mptViewer线程运行Viewer::Run函数实现显示功能。因此，找到/src/Viewer.cc文件，其中定义了Viewer::Run函数：
void Viewer::Run() { ... }
其中，以下这段代码是处理标注过特征点的图像。显然，toShow变量就是存储了处理完成的图像。
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);以下这段代码是处理点云图显示的。然而，我们并不能从这里获取点云数据，因为算法利用了DrawMapPoints函数完成了点云的绘制。
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(); // 画出点云因此，找到/src/MapDrawer.cc文件，找到DrawMapPoints函数：
void MapDrawer::DrawMapPoints() { ... }其中，这一部分实现了点云的绘制：
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));
}③ 应用方法
当一帧图像输入到节点后，会调用TrackMonocular函数完成视觉SLAM（以ros_mono.cc为例）：
mpSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec());由于ORB_SLAM编译后生成的是lib文件，因此难以在其中嵌入ROS。考虑到需要将数据输出，可以考虑改写TrackMonocular函数，使得函数能够返回点云、图像等数据。
2. 利用pcl与ros实现点云转存与ROS消息发布
① 改写ORB_SLAM3
将点云数据及图像数据存储到一个全局变量中，然后用修改后的TrackMonocular函数（TrackMonocularOutput函数）返回结构体，结构体中包含相机追踪数据、点云数据、图像数据。
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.修改函数void MapDrawer::DrawMapPoints()
修改函数中以下for循环：
// 修改前
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));
}在遍历点云中的点时，存入pcl点云变量。修改后如下：
// 创建点云图
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));
// 将点插入点云图
pcl::PointXYZ point;
point.x = pos(0);
point.y = pos(1);
point.z = pos(2);
point_cloud->points.push_back(point);
}
// TEST 保存点云图像到本地
//if(point_cloud->points.size())
// pcl::io::savePCDFileBinary("orb_slam3.pcd", *point_cloud);这里，可以直接将point_cloud变量存储到全局变量中。这里我的处理方法是，修改函数定义，让其返回 pcl::PointCloud<pcl::PointXYZ>::Ptr 数据。修改好的函数如下：
pcl::PointCloud<pcl::PointXYZ>::Ptr MapDrawer::DrawMapPoints()
{
// 创建点云图
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));
// 将点插入点云图
pcl::PointXYZ point;
point.x = pos(0);
point.y = pos(1);
point.z = pos(2);
point_cloud->points.push_back(point);
}
// 保存点云图像到本地
//if(point_cloud->points.size())
// pcl::io::savePCDFileBinary("orb_slam3.pcd", *point_cloud);
glEnd();
return point_cloud;
}c.修改函数申明
在修改函数返回值后，需要在/include/MapDrawer.h文件中修改函数申明：
pcl::PointCloud<pcl::PointXYZ>::Ptr DrawMapPoints();（如果直接将变量存入全局变量则无需这一步操作）
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() 函数
原始点云显示代码如下，原始的DrawMapPoints函数没有返回值。
if(menuShowPoints)
mpMapDrawer->DrawMapPoints();将其修改为以下样式，用全局变量G_pcToShow接收返回的点云数据
if(menuShowPoints)
G_pcToShow = mpMapDrawer->DrawMapPoints(); // 处理后的点云显示
// 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.定义一个结构体变量，包含点云、图像、追踪信息
// 利用结构体返回位置、点云、图像（在 namespace ORB_SLAM3 下）
typedef struct {
pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud;
cv::Mat image;
Sophus::SE3f pos;
}SLAM_Output;
c.申明新定义的函数
// 返回相机追踪信息、点云数据、图像数据（位置在原TrackMonocular函数之后即可）
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>
// 全局变量（用于传出输出点云与输出图像）
pcl::PointCloud<pcl::PointXYZ>::Ptr G_pcToShow(new pcl::PointCloud<pcl::PointXYZ>());
cv::Mat G_imToShow;b.修改TrackMonocular函数，形成新函数
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
{
// 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱
unique_lock<mutex> lock(mMutexMode);
// mbActivateLocalizationMode为true会关闭局部地图线程，仅跟踪模式
if(mbActivateLocalizationMode)
{
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{
usleep(1000);
}
// 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新
mpTracker->InformOnlyTracking(true);
// 关闭线程可以使得别的线程得到更多的资源
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;
}
// 如果检测到重置活动地图的标志为true,将重置地图
else if(mbResetActiveMap)
{
cout << "SYSTEM -> Reseting active map in monocular case" << endl;
mpTracker->ResetActiveMap();
mbResetActiveMap = false;
}
}
// 如果是单目VIO模式，把IMU数据存储到队列mlQueueImuData
if (mSensor == System::IMU_MONOCULAR)
for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
mpTracker->GrabImuData(vImuMeas[i_imu]);
// 计算相机位姿
Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename);
// 更新跟踪状态和参数
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库，在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应用层
i. 修改ros_mono.cc
ROS节点改写后如下：
#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; // 创建SLMA系统指针
};
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接收消息回调函数
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系统追踪单目相机 返回相机位置数据
ORB_SLAM3::SLAM_Output data;
data = mpSLAM->TrackMonocularOutput(cv_ptr->image, cv_ptr->header.stamp.toSec());
// 发布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;
}
// 发布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)); //此行会莫名其妙导致运行<Tracking.cc>623行报错退出
}应用层中，主要是调用了上一步已经修改好的TrackMonocularOutput函数替换原来使用的TrackMonocular函数，并将返回值通过ROS发布。
ii. 修改CMakeList.txt
需要在其中添加PCL库，在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转ROSMSG PointCloud2：ROS与PCL中各种点云数据格式之间的转换（大总结）_pcl::torosmsg-CSDN博客文章浏览阅读2.3k次，点赞11次，收藏63次。ROS与PCL中各种点云数据格式之间的转换（大总结）三种常用点云数据格式：pcl::PointCloud< PointT>pcl::PCLPointCloud2snesor_msgs::PointCloud21.sensor_msgs::PointCloud2转pcl::PCLPointCloud2pcl_conversion::toPCl(sensor_msgs::PointCloud2,pcl::PCLPointCloud2)2.sensor_msgs::PointClo_pcl::torosmsghttps://blog.csdn.net/m0_45388819/article/details/113794706OpenCV转ROSMSG Image：利用opencv将本地图片转换成ROS格式_opencv 图像类型转换为rosmsg类型-CSDN博客文章浏览阅读884次。转自:http://blog.csdn.net/yake827/article/details/44593621本文主要讲解如何将本地的图片通过ROS来显示出来。主要利用了OpenCV库，一样是来源于ROS官网.创建一个ROS工作区工作区还是存放和编译我们的文件[plain] view plain copy$ mkdi_opencv 图像类型转换为rosmsg类型https://blog.csdn.net/mxgsgtc/article/details/72143054将ORBSLAM中的Map转换为点云：ORB SLAM3 点云地图保存_orbslam3 点云-CSDN博客文章浏览阅读5.1k次，点赞17次，收藏90次。简单修改ORB_SLAM3源码， 使其能保存pcd格式的点云地图。_orbslam3 点云https://blog.csdn.net/qq_43591054/article/details/125693886
3. 运行
使用 <三、3> 介绍的步骤运行即可，可以在其他节点接收点云消息和图像消息。
正常运行后，可以通过rqt_image_view来查看图像数据是否正确发布：
可以通过 rostopic echo / rostopic hz 来查看点云数据是否正确发布。如需要通过rviz来查看点云数据是否正确，则需要新增一个tf转换（上面代码中，点云的frame_id为camera）。
改写双目与RGB-D方法与单目相同，因此不再赘述。
五、附录
附录一：测试使用的无人机模型文件
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>
附录二：无人机测试环境及launch文件
map_maze_target.world文章来源：https://www.toymoban.com/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文章来源地址https://www.toymoban.com/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 -->到了这里，关于ORB_SLAM3配置及修改——将图像、点云用ROS消息发布（基于无人机仿真）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
