ORB_SLAM3配置及修改——将图像、点云用ROS消息发布（基于无人机仿真）

Wang.Shujie

已于 2024-03-27 11:04:29 修改

阅读量2.3k

点赞数 60

分类专栏： ORB_SLAM ros 文章标签：无人机 opencv 图像处理自动驾驶

于 2024-03-13 19:10:14 首次发布

本文链接：https://blog.csdn.net/Und_waasj/article/details/136659513

版权

ros 同时被 2 个专栏收录

4 篇文章 1 订阅

订阅专栏

ORB_SLAM

2 篇文章 1 订阅

订阅专栏

本文有点长，可以根据目录跳转到想看的部分。因为仿真和应用环境不同，可能例程的运行方式（输入话题等）有所不同，但第三部分有关ORB_SLAM3相机仿真标定、第四部分有关ORB_SLAM3源码修改的部分是通用的。

iv.标定数据转换为orb_slam3的相机标定数据

2. 修改源码

3. 运行ROS例程

四、修改ORB_SLAM3源码实现点云、图像的ROS消息输出

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函数，形成新函数

一、仿真环境配置

采用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.sh

3. 测试

可以使用数据集测试，我们这边用了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.launch

ii. 启动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 &timestamp, 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 &timestamp, 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

<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

<?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  -->