devbox
小小林熬夜学编程
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

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

作者：小小林熬夜学编程 | 2024-04-19 20:24:57

赞

踩

orb-slam3的点云

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

目录

一、仿真环境配置

1.双系统安装

① 工具准备

② 启动盘制作

    i. 格式化U盘

    ii. 制作启动盘

③ 系统安装

2. XTDrone仿真系统安装

二、ORB_SLAM3源码下载及编译

1. 源码下载

2. 编译

① ORB_SLAM3编译

② ROS例程编译

3. 测试

① realsense2安装

② realsense_ros安装

③ 重新编译ORB_SLAM3

④ 运行例程

三、ROS例程的运行与修改

1. 相机标定

① 制作标定世界

②制作launch文件

③ 运行世界并完成标定

    i. 启动Gazebo仿真

    ii. 启动XTDrone无人机控制模块

    iii. 启动标定节点完成标定

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

2. 修改源码

3. 运行ROS例程

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

1. 源码分析

① 初始化过程

② 显示输出过程

③ 应用方法

2. 利用pcl与ros实现点云转存与ROS消息发布

① 改写ORB_SLAM3

    i.修改MapDrawer.cc

        a.添加PCL头文件

        b.修改函数void MapDrawer::DrawMapPoints()

        c.修改函数申明

    ii.修改Viewer.cc

        a.添加PCL头文件，引用外部定义的全局变量

        b.修改 void Viewer::Run() 函数

    iii.修改system.h

        a.添加PCL头文件

         b.定义一个结构体变量，包含点云、图像、追踪信息

         c.申明新定义的函数

    iv. 修改system.cc

        a.添加PCL头文件，定义全局变量

        b.修改TrackMonocular函数，形成新函数

        c.修改CMakeList.txt

② 改写ROS应用层

    i. 修改ros_mono.cc

    ii. 修改CMakeList.txt

3. 运行

五、附录

附录一：测试使用的无人机模型文件

附录二：无人机测试环境及launch文件

一、仿真环境配置

        采用XTDrone实现无人机Gazebo仿真。

1.双系统安装

① 工具准备

  • Rufus软件

        点击链接下载安装Rufus软件，选择图示版本。

  • Usb3.0接口U盘一个，推荐容量大于16G
  • Ubuntu18.04桌面版镜像文件

         点击链接下载安装Ubuntu18.04系统镜像，选择桌面版镜像，并设置好下载路径确保能找到镜像。

image.png

② 启动盘制作

    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_SLAM3icon-default.png?t=N7T8https://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.icon-default.png?t=N7T8https://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中有写明。

        配置完依赖后，执行以下代码进行编译。

  1. cd ORB_SLAM3
  2. chmod +x build.sh
  3. ./build.sh

        成功编译完成后，会在/ORB_SLAM3/lib/目录下生成libORB_SLAM.so文件。

② ROS例程编译

        首先将以下代码加入~/.bashrc中。

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS

         随后运行：

  1. chmod +x build_ros.sh
  2. ./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：

  1. # 依赖
  2. sudo apt-get install libudev-dev pkg-config libgtk-3-dev
  3. sudo apt-get install libusb-1.0-0-dev pkg-config
  4. sudo apt-get install libglfw3-dev
  5. sudo apt-get install libssl-dev
  6. ./scripts/setup_udev_rules.sh
  7. ./scripts/patch-realsense-ubuntu-lts.sh
  8. # 编译
  9. mkdir build
  10. cd build
  11. cmake ../ -DBUILD_EXAMPLES=true
  12. make
  13. 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），编译过程如下：

  1. mkdir -p realsense_ros_ws/src
  2. cd realsense_ros_ws/src        # 复制解压好的realsense-ros-2.3.2到此目录下
  3. catkin_init_workspace
  4. cd ..
  5. catkin_make clean
  6. catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
  7. catkin_make install
  8. echo "source ~/YOUR_WORKSAPCE/devel/setup.bash" >> ~/.bashrc
  9. source ~/.bashrc
  10. # 测试运行
  11. roslaunch realsense2_camera rs_t265.launch

③ 重新编译ORB_SLAM3

        因为ORB_SLAM3在编译时检测到未安装realsense2，则不会编译相关例程。所以，安装测试号realsense2后，重新编译一次即可。

④ 运行例程

  1. # T265 ORBSLAM3
  2. cd <your_dir>/ORB_SLAM3
  3. ./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如下：

  1. <sdf version='1.6'>
  2.   <world name='default'>
  3.  
  4.     <light name='sun' type='directional'>
  5.       <cast_shadows>1</cast_shadows>
  6.       <pose frame=''>0 0 10 0 -0 0</pose>
  7.       <diffuse>0.8 0.8 0.8 1</diffuse>
  8.       <specular>0.2 0.2 0.2 1</specular>
  9.       <attenuation>
  10.         <range>1000</range>
  11.         <constant>0.9</constant>
  12.         <linear>0.01</linear>
  13.         <quadratic>0.001</quadratic>
  14.       </attenuation>
  15.       <direction>-0.5 0.1 -0.9</direction>
  16.     </light>
  17.  
  18.     <!-- 无人机 -->
  19.     <model name='ground_plane'>
  20.       <static>1</static>
  21.       <link name='link'>
  22.         <collision name='collision'>
  23.           <geometry>
  24.             <plane>
  25.               <normal>0 0 1</normal>
  26.               <size>300 300</size>
  27.             </plane>
  28.           </geometry>
  29.           <surface>
  30.             <friction>
  31.               <ode>
  32.                 <mu>100</mu>
  33.                 <mu2>50</mu2>
  34.               </ode>
  35.               <torsional>
  36.                 <ode/>
  37.               </torsional>
  38.             </friction>
  39.             <contact>
  40.               <ode/>
  41.             </contact>
  42.             <bounce/>
  43.           </surface>
  44.           <max_contacts>10</max_contacts>
  45.         </collision>
  46.         <visual name='visual'>
  47.           <cast_shadows>0</cast_shadows>
  48.           <geometry>
  49.             <plane>
  50.               <normal>0 0 1</normal>
  51.               <size>300 300</size>
  52.             </plane>
  53.           </geometry>
  54.           <material>
  55.             <script>
  56.               <uri>file://media/materials/scripts/gazebo.material</uri>
  57.               <name>Gazebo/Grey</name>
  58.             </script>
  59.           </material>
  60.         </visual>
  61.         <self_collide>0</self_collide>
  62.         <enable_wind>0</enable_wind>
  63.         <kinematic>0</kinematic>
  64.       </link>
  65.     </model>
  66.  
  67.     <gravity>0 0 -9.8</gravity>
  68.     <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
  69.     <atmosphere type='adiabatic'/>
  70.     <physics name='default_physics' default='0' type='ode'>
  71.       <max_step_size>0.001</max_step_size>
  72.       <real_time_factor>1</real_time_factor>
  73.       <real_time_update_rate>1000</real_time_update_rate>
  74.     </physics>
  75.     <scene>
  76.       <ambient>0.4 0.4 0.4 1</ambient>
  77.       <background>0.7 0.7 0.7 1</background>
  78.       <shadows>1</shadows>
  79.     </scene>
  80.     <wind/>
  81.  
  82.     <spherical_coordinates>
  83.       <surface_model>EARTH_WGS84</surface_model>
  84.       <latitude_deg>0</latitude_deg>
  85.       <longitude_deg>0</longitude_deg>
  86.       <elevation>0</elevation>
  87.       <heading_deg>0</heading_deg>
  88.     </spherical_coordinates>
  89.  
  90.     <!-- 起飞标志点（非必须） -->
  91.     <include>
  92.       <uri>model://takeoff</uri>
  93.       <pose>-0 -0.5 0 0 1.57 1.57</pose>
  94.     </include>
  95.  
  96.     <!-- 标定板（注意，此标定板为单面的模型，从背面看为透明） -->
  97.     <include>
  98.       <uri>model://checkerboard_plane</uri>
  99.       <pose>5 0 2 0 1.57 3.14</pose>
  100.     </include>
  101.  
  102.     <state world_name='default'>
  103.       <sim_time>135 449000000</sim_time>
  104.       <real_time>136 177712649</real_time>
  105.       <wall_time>1656128132 28380767</wall_time>
  106.       <iterations>135449</iterations>
  107.       <model name='ground_plane'>
  108.         <pose frame=''>0 0 0 0 -0 0</pose>
  109.         <scale>1 1 1</scale>
  110.         <link name='link'>
  111.           <pose frame=''>0 0 0 0 -0 0</pose>
  112.           <velocity>0 0 0 0 -0 0</velocity>
  113.           <acceleration>0 0 0 0 -0 0</acceleration>
  114.           <wrench>0 0 0 0 -0 0</wrench>
  115.         </link>
  116.       </model>
  117.       <light name='sun'>
  118.         <pose frame=''>0 0 10 0 -0 0</pose>
  119.       </light>
  120.     </state>
  121.     <gui fullscreen='0'>
  122.       <camera name='user_camera'>
  123.         <pose frame=''>-1.41507 9.30903 47.355 -0 1.4338 -1.39902</pose>
  124.         <view_controller>orbit</view_controller>
  125.         <projection_type>perspective</projection_type>
  126.       </camera>
  127.     </gui>
  128.   </world>
  129. </sdf>

②制作launch文件

        launch文件的制作比较简单，可以直接参考XTDrone的其他启动节点。修改后的calibration.launch文件如下：

  1. <?xml version="1.0"?>
  2. <launch>
  3.     <!-- MAVROS posix SITL environment launch script -->
  4.     <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
  5.     <!-- vehicle model and world -->
  6.     <arg name="est" default="ekf2"/>
  7.     <arg name="world" default="/home/xtdrone/user_ws/modules/slam/Gazebo_Calibration/calibration.world"/>
  8.     <!-- gazebo configs -->
  9.     <arg name="gui" default="true"/>
  10.     <arg name="debug" default="false"/>
  11.     <arg name="verbose" default="false"/>
  12.     <arg name="paused" default="false"/>
  13.     <!-- Gazebo sim -->
  14.     <include file="$(find gazebo_ros)/launch/empty_world.launch">
  15.         <arg name="gui" value="$(arg gui)"/>
  16.         <arg name="world_name" value="$(arg world)"/>
  17.         <arg name="debug" value="$(arg debug)"/>
  18.         <arg name="verbose" value="$(arg verbose)"/>
  19.         <arg name="paused" value="$(arg paused)"/>
  20.     </include>
  21.      <!-- iris_0 -->
  22.      <group ns="iris_0">
  23.         <!-- MAVROS and vehicle configs -->
  24.             <arg name="ID" value="0"/>
  25.             <arg name="ID_in_group" value="0"/>
  26.             <arg name="fcu_url" default="udp://:24540@localhost:34580"/>
  27.         <!-- PX4 SITL and vehicle spawn -->
  28.         <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
  29.             <arg name="x" value="0"/>
  30.             <arg name="y" value="0"/>
  31.             <arg name="z" value="0.5"/>
  32.             <arg name="R" value="0"/>
  33.             <arg name="P" value="0"/>
  34.             <arg name="Y" value="0"/>
  35.             <arg name="vehicle" value="iris"/>
  36.             <arg name="sdf" value="iris_zhihang"/>
  37.             <arg name="mavlink_udp_port" value="18570"/>
  38.             <arg name="mavlink_tcp_port" value="4560"/>
  39.             <arg name="ID" value="$(arg ID)"/>
  40.             <arg name="ID_in_group" value="$(arg ID_in_group)"/>
  41.         </include>
  42.         <!-- MAVROS -->
  43.         <include file="$(find mavros)/launch/px4.launch">
  44.             <arg name="fcu_url" value="$(arg fcu_url)"/>
  45.             <arg name="gcs_url" value=""/>
  46.             <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
  47.             <arg name="tgt_component" value="1"/>
  48.         </include>
  49.     </group>
  50. </launch>
  51. <!--the launch file is generated by XTDrone multi-vehicle generator.py  -->

        其中第七行为上一步calibration.world的存储地址。

③ 运行世界并完成标定

    i. 启动Gazebo仿真
  1. cd ~/user_ws/modules/slam/Gazebo_Calibration    # 进入到存储launch文件的目录下
  2. roslaunch ./calibration.launch
    ii. 启动XTDrone无人机控制模块
  1. # 无人机通讯控制节点
  2. cd ~/XTDrone/communication/
  3. python multirotor_communication.py iris 0
  1. # 用键盘控制无人机飞行
  2. cd ~/XTDrone/control/keyboard
  3. 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，我标定完数据如下：

  1. image_width: 1280
  2. image_height: 720
  3. camera_name: narrow_stereo
  4. camera_matrix:
  5.   rows: 3
  6.   cols: 3
  7.   data: [1109.51051,    0.     ,  637.897  ,
  8.             0.     , 1109.55019,  358.77075,
  9.             0.     ,    0.     ,    1.     ]
  10. distortion_model: plumb_bob
  11. distortion_coefficients:
  12.   rows: 1
  13.   cols: 5
  14.   data: [0.000544, -0.003422, -0.000323, -0.000315, 0.000000]
  15. rectification_matrix:
  16.   rows: 3
  17.   cols: 3
  18.   data: [1., 0., 0.,
  19.          0., 1., 0.,
  20.          0., 0., 1.]
  21. projection_matrix:
  22.   rows: 3
  23.   cols: 4
  24.   data: [1109.35156,    0.     ,  637.53416,    0.     ,
  25.             0.     , 1109.77466,  358.61387,    0.     ,
  26.             0.     ,    0.     ,    1.     ,    0.     ]

        复制/ORB_SLAM3/Example/ROS/ORB_SLAM3/Asus.yaml文件，并对其进行修改，修改方法如下：

  1. %YAML:1.0
  2.  
  3. #--------------------------------------------------------------------------------------------
  4. # Camera Parameters. Adjust them!
  5. #--------------------------------------------------------------------------------------------
  6. File.version: "1.0"
  7.  
  8. Camera.type: "PinHole"    # 针孔相机
  9.  
  10. # Camera calibration and distortion parameters (OpenCV) 
  11. Camera1.fx: 1109.35156    # 对应projection_matrix.data的第1个参数
  12. Camera1.fy: 1109.77466    # 对应projection_matrix.data的第6个参数
  13. Camera1.cx: 637.53416     # 对应projection_matrix.data的第3个参数
  14. Camera1.cy: 358.61387     # 对应projection_matrix.data的第7个参数
  15.  
  16. # 畸变纠正，对应distortion_coefficients矩阵
  17. Camera1.k1: 0.000544
  18. Camera1.k2: -0.003422
  19. Camera1.p1: -0.000323
  20. Camera1.p2: -0.000315
  21. Camera1.k3: 0.000000
  22.  
  23. # Camera frames per second 
  24. Camera.fps: 30
  25.  
  26. # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
  27. Camera.RGB: 0
  28.  
  29. # Camera resolution
  30. Camera.width: 1280        # 对应image_width
  31. Camera.height: 720        # 对应image_height
  32.  
  33. #--------------------------------------------------------------------------------------------
  34. # ORB Parameters 以下为ORB的参数，基本不需要修改
  35. #--------------------------------------------------------------------------------------------
  36.  
  37. # ORB Extractor: Number of features per image
  38. ORBextractor.nFeatures: 1000
  39.  
  40. # ORB Extractor: Scale factor between levels in the scale pyramid 	
  41. ORBextractor.scaleFactor: 1.2
  42.  
  43. # ORB Extractor: Number of levels in the scale pyramid	
  44. ORBextractor.nLevels: 8
  45.  
  46. # ORB Extractor: Fast threshold
  47. # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
  48. # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
  49. # You can lower these values if your images have low contrast			
  50. ORBextractor.iniThFAST: 20
  51. ORBextractor.minThFAST: 7
  52.  
  53. #--------------------------------------------------------------------------------------------
  54. # Viewer Parameters
  55. #--------------------------------------------------------------------------------------------
  56. Viewer.KeyFrameSize: 0.05
  57. Viewer.KeyFrameLineWidth: 1.0
  58. Viewer.GraphLineWidth: 0.9
  59. Viewer.PointSize: 2.0
  60. Viewer.CameraSize: 0.08
  61. Viewer.CameraLineWidth: 3.0
  62. Viewer.ViewpointX: 0.0
  63. Viewer.ViewpointY: -0.7
  64. Viewer.ViewpointZ: -1.8
  65. Viewer.ViewpointF: 500.0
  66.

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”：

  1. #define cfg_ROS_IMG_INPUT_TOPIC     "/iris_0/realsense/depth_camera/color/image_raw"
  2.  
  3. ros::Subscriber sub = nodeHandler.subscribe(cfg_ROS_IMG_INPUT_TOPIC, 1, &ImageGrabber::GrabImage, &igb);

3. 运行ROS例程

        重新编译后，运行（其中depth_camera.yaml是上一步标定完成后的标定文件）：

  1. # 启动一个无人机飞行场景
  2. roslaunch px4 zhihang2.launch
  3.  
  4. # 启动控制及通讯节点
  5. cd ~/user_ws/modules/gui/scripts
  6. python multirotor_communication.py iris 0
  7. cd ~/XTDrone/control/keyboard
  8. python multirotor_keyboard_control.py iris 0 vel
  9.  
  10. # 启动ORB_SLAM3
  11. 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下的初始化函数：

  1. System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
  2.                const bool bUseViewer, const int initFr, const string &strSequence):
  3.     mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false), mbResetActiveMap(false),
  4.     mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), mbShutDown(false)
  5. { ... }

        在这个函数中，会完成导入数据、开启Tracking线程、LocalMapping线程、LoopClosing线程，开启显示线程。其中，我使用的方法是通过显示线程将数据导出。

        在该函数的最后，有创建显示线程的代码：

  1.     //Initialize the Viewer thread and launch    创建并开启显示线程
  2.     if(bUseViewer)
  3.     //if(false) // TODO
  4.     {
  5.         mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile,settings_);
  6.         mptViewer = new thread(&Viewer::Run, mpViewer);
  7.         mpTracker->SetViewer(mpViewer);
  8.         mpLoopCloser->mpViewer = mpViewer;
  9.         mpViewer->both = mpFrameDrawer->both;
  10.     }
  11.

② 显示输出过程

        显而易见，算法通过mptViewer线程运行Viewer::Run函数实现显示功能。因此，找到/src/Viewer.cc文件，其中定义了Viewer::Run函数：

void Viewer::Run() { ... }

        其中，以下这段代码是处理标注过特征点的图像。显然，toShow变量就是存储了处理完成的图像。

  1. cv::Mat toShow;
  2. cv::Mat im = mpFrameDrawer->DrawFrame(trackedImageScale);
  3.  
  4. if(both){
  5.     cv::Mat imRight = mpFrameDrawer->DrawRightFrame(trackedImageScale);
  6.     cv::hconcat(im,imRight,toShow);
  7. }
  8. else{
  9.     toShow = im;
  10. }
  11.  
  12. if(mImageViewerScale != 1.f) {
  13.     int width = toShow.cols * mImageViewerScale;
  14.     int height = toShow.rows * mImageViewerScale;
  15.     cv::resize(toShow, toShow, cv::Size(width, height));
  16. }
  17.  
  18. cv::imshow("ORB-SLAM3: Current Frame",toShow);
  19. cv::waitKey(mT);

        以下这段代码是处理点云图显示的。然而，我们并不能从这里获取点云数据，因为算法利用了DrawMapPoints函数完成了点云的绘制。

  1. d_cam.Activate(s_cam);
  2. glClearColor(1.0f,1.0f,1.0f,1.0f);
  3. mpMapDrawer->DrawCurrentCamera(Twc);
  4. if(menuShowKeyFrames || menuShowGraph || menuShowInertialGraph || menuShowOptLba)
  5.     mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph, menuShowInertialGraph, menuShowOptLba);
  6. if(menuShowPoints)
  7.     mpMapDrawer->DrawMapPoints();    // 画出点云

        因此，找到/src/MapDrawer.cc文件，找到DrawMapPoints函数：

void MapDrawer::DrawMapPoints() { ... }

        其中，这一部分实现了点云的绘制：

  1. for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
  2. {
  3.     if((*sit)->isBad())
  4.         continue;
  5.     Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
  6.     glVertex3f(pos(0),pos(1),pos(2));
  7. }

③ 应用方法

        当一帧图像输入到节点后，会调用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头文件
  1. #include <pcl/point_types.h>
  2. #include <pcl/point_cloud.h>
  3. #include <pcl/io/pcd_io.h>
  4. #include <pcl_conversions/pcl_conversions.h>
        b.修改函数void MapDrawer::DrawMapPoints()

        修改函数中以下for循环：

  1. // 修改前
  2. for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
  3. {
  4.     if((*sit)->isBad())
  5.         continue;
  6.     Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
  7.     glVertex3f(pos(0),pos(1),pos(2));
  8. }

         在遍历点云中的点时，存入pcl点云变量。修改后如下：

  1. // 创建点云图
  2. pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
  3.  
  4.  
  5. for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
  6. {
  7.     if((*sit)->isBad())
  8.         continue;
  9.     Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
  10.     glVertex3f(pos(0),pos(1),pos(2));
  11.  
  12.     // 将点插入点云图
  13.     pcl::PointXYZ point;
  14.     point.x = pos(0);
  15.     point.y = pos(1);
  16.     point.z = pos(2);
  17.     point_cloud->points.push_back(point);
  18. }
  19.  
  20. // TEST 保存点云图像到本地
  21. //if(point_cloud->points.size())
  22. //    pcl::io::savePCDFileBinary("orb_slam3.pcd", *point_cloud);

        这里，可以直接将point_cloud变量存储到全局变量中。这里我的处理方法是，修改函数定义，让其返回 pcl::PointCloud<pcl::PointXYZ>::Ptr 数据。修改好的函数如下：

  1. pcl::PointCloud<pcl::PointXYZ>::Ptr MapDrawer::DrawMapPoints()
  2. {
  3.     // 创建点云图
  4.     pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
  5.  
  6.     Map* pActiveMap = mpAtlas->GetCurrentMap();
  7.     if(!pActiveMap)
  8.         return point_cloud;
  9.  
  10.     const vector<MapPoint*> &vpMPs = pActiveMap->GetAllMapPoints();
  11.     const vector<MapPoint*> &vpRefMPs = pActiveMap->GetReferenceMapPoints();
  12.  
  13.     set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());
  14.  
  15.     if(vpMPs.empty())
  16.         return point_cloud;
  17.  
  18.     glPointSize(mPointSize);
  19.     glBegin(GL_POINTS);
  20.     glColor3f(0.0,0.0,0.0);
  21.  
  22.     for(size_t i=0, iend=vpMPs.size(); i<iend;i++)
  23.     {
  24.         if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i]))
  25.             continue;
  26.         Eigen::Matrix<float,3,1> pos = vpMPs[i]->GetWorldPos();
  27.         glVertex3f(pos(0),pos(1),pos(2));
  28.     }
  29.     glEnd();
  30.  
  31.     glPointSize(mPointSize);
  32.     glBegin(GL_POINTS);
  33.     glColor3f(1.0,0.0,0.0);
  34.  
  35.     for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
  36.     {
  37.         if((*sit)->isBad())
  38.             continue;
  39.         Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
  40.         glVertex3f(pos(0),pos(1),pos(2));
  41.  
  42.         // 将点插入点云图
  43.         pcl::PointXYZ point;
  44.         point.x = pos(0);
  45.         point.y = pos(1);
  46.         point.z = pos(2);
  47.         point_cloud->points.push_back(point);
  48.     }
  49.  
  50.     // 保存点云图像到本地
  51.     //if(point_cloud->points.size())
  52.     //    pcl::io::savePCDFileBinary("orb_slam3.pcd", *point_cloud);
  53.  
  54.     glEnd();
  55.  
  56.     return point_cloud;
  57. }
        c.修改函数申明

        在修改函数返回值后，需要在/include/MapDrawer.h文件中修改函数申明：

pcl::PointCloud<pcl::PointXYZ>::Ptr DrawMapPoints();

        （如果直接将变量存入全局变量则无需这一步操作）

    ii.修改Viewer.cc
        a.添加PCL头文件，引用外部定义的全局变量
  1. #include <pcl/point_types.h>
  2. #include <pcl/point_cloud.h>
  3. #include <pcl/io/pcd_io.h>
  4. #include <pcl_conversions/pcl_conversions.h>
  5.  
  6. // 全局变量定义在/src/system.cc
  7. // 本文 <四.2.iv.b> 中介绍
  8. extern pcl::PointCloud<pcl::PointXYZ>::Ptr G_pcToShow;
  9. extern cv::Mat G_imToShow;
        b.修改 void Viewer::Run() 函数

        原始点云显示代码如下，原始的DrawMapPoints函数没有返回值。

  1. if(menuShowPoints)
  2.     mpMapDrawer->DrawMapPoints();

        将其修改为以下样式，用全局变量G_pcToShow接收返回的点云数据

  1. if(menuShowPoints)
  2.     G_pcToShow = mpMapDrawer->DrawMapPoints();                              // 处理后的点云显示
  3.     // TEST
  4.     //if(G_pcToShow->points.size())
  5.     //    pcl::io::savePCDFileBinary("orb_slam3.pcd", *G_pcToShow);

        原始图片显示代码如下：

  1. cv::Mat toShow;
  2. cv::Mat im = mpFrameDrawer->DrawFrame(trackedImageScale);
  3.  
  4. if(both){
  5.     cv::Mat imRight = mpFrameDrawer->DrawRightFrame(trackedImageScale);
  6.     cv::hconcat(im,imRight,toShow);
  7. }
  8. else{
  9.     toShow = im;
  10. }
  11.  
  12. if(mImageViewerScale != 1.f){
  13.     int width = toShow.cols * mImageViewerScale;
  14.     int height = toShow.rows * mImageViewerScale;
  15.     cv::resize(toShow, toShow, cv::Size(width, height));
  16. }
  17.  
  18. cv::imshow("ORB-SLAM3: Current Frame",toShow);
  19. cv::waitKey(mT);

        修改将其接受图片的变量换为全局变量G_imToShow，修改后如下：

  1. cv::Mat im = mpFrameDrawer->DrawFrame(trackedImageScale);
  2.  
  3. if(both){
  4.     cv::Mat imRight = mpFrameDrawer->DrawRightFrame(trackedImageScale);
  5.     cv::hconcat(im,imRight,G_imToShow);
  6. }
  7. else{
  8.     G_imToShow = im;
  9. }
  10.  
  11. if(mImageViewerScale != 1.f){
  12.     int width = G_imToShow.cols * mImageViewerScale;
  13.     int height = G_imToShow.rows * mImageViewerScale;
  14.     cv::resize(G_imToShow, G_imToShow, cv::Size(width, height));
  15. }
  16.  
  17. cv::imshow("ORB-SLAM3: Current Frame",G_imToShow);                        // 处理后的视频流显示
  18. cv::waitKey(mT);
    iii.修改system.h
        a.添加PCL头文件
  1. #include <pcl/point_types.h>
  2. #include <pcl/point_cloud.h>
  3. #include <pcl/io/pcd_io.h>
  4. #include <pcl_conversions/pcl_conversions.h>
         b.定义一个结构体变量，包含点云、图像、追踪信息
  1. // 利用结构体返回位置、点云、图像（在 namespace ORB_SLAM3 下）
  2. typedef struct {
  3.     pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud;
  4.     cv::Mat image;
  5.     Sophus::SE3f pos;
  6. }SLAM_Output;
         c.申明新定义的函数
  1. // 返回相机追踪信息、点云数据、图像数据（位置在原TrackMonocular函数之后即可）
  2. 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头文件，定义全局变量
  1. #include <pcl/point_types.h>
  2. #include <pcl/point_cloud.h>
  3. #include <pcl/io/pcd_io.h>
  4. #include <pcl_conversions/pcl_conversions.h>
  5.  
  6. // 全局变量（用于传出输出点云与输出图像）
  7. pcl::PointCloud<pcl::PointXYZ>::Ptr G_pcToShow(new pcl::PointCloud<pcl::PointXYZ>());
  8. cv::Mat G_imToShow;
        b.修改TrackMonocular函数，形成新函数
  1. SLAM_Output System::TrackMonocularOutput(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename)
  2. {
  3.     SLAM_Output data;
  4.  
  5.     {
  6.         unique_lock<mutex> lock(mMutexReset);
  7.         if(mbShutDown)
  8.             return data;
  9.     }
  10.     // 确保是单目或单目VIO模式
  11.     if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR)
  12.     {
  13.         cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular nor Monocular-Inertial." << endl;
  14.         exit(-1);
  15.     }
  16.  
  17.     cv::Mat imToFeed = im.clone();
  18.     if(settings_ && settings_->needToResize()){
  19.         cv::Mat resizedIm;
  20.         cv::resize(im,resizedIm,settings_->newImSize());
  21.         imToFeed = resizedIm;
  22.     }
  23.  
  24.     // Check mode change
  25.     {
  26.         // 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱
  27.         unique_lock<mutex> lock(mMutexMode);
  28.         // mbActivateLocalizationMode为true会关闭局部地图线程，仅跟踪模式
  29.         if(mbActivateLocalizationMode)
  30.         {
  31.             mpLocalMapper->RequestStop();
  32.  
  33.             // Wait until Local Mapping has effectively stopped
  34.             while(!mpLocalMapper->isStopped())
  35.             {
  36.                 usleep(1000);
  37.             }
  38.             // 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新
  39.             mpTracker->InformOnlyTracking(true);
  40.             // 关闭线程可以使得别的线程得到更多的资源
  41.             mbActivateLocalizationMode = false;
  42.         }
  43.         if(mbDeactivateLocalizationMode)
  44.         {
  45.             mpTracker->InformOnlyTracking(false);
  46.             mpLocalMapper->Release();
  47.             mbDeactivateLocalizationMode = false;
  48.         }
  49.     }
  50.  
  51.     // Check reset
  52.     {
  53.         unique_lock<mutex> lock(mMutexReset);
  54.         if(mbReset)
  55.         {
  56.             mpTracker->Reset();
  57.             mbReset = false;
  58.             mbResetActiveMap = false;
  59.         }
  60.         // 如果检测到重置活动地图的标志为true,将重置地图
  61.         else if(mbResetActiveMap)
  62.         {
  63.             cout << "SYSTEM -> Reseting active map in monocular case" << endl;
  64.             mpTracker->ResetActiveMap();
  65.             mbResetActiveMap = false;
  66.         }
  67.     }
  68.     // 如果是单目VIO模式，把IMU数据存储到队列mlQueueImuData
  69.     if (mSensor == System::IMU_MONOCULAR)
  70.         for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
  71.             mpTracker->GrabImuData(vImuMeas[i_imu]);
  72.  
  73.     // 计算相机位姿
  74.     Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename);
  75.  
  76.     // 更新跟踪状态和参数
  77.     unique_lock<mutex> lock2(mMutexState);
  78.     mTrackingState = mpTracker->mState;
  79.     mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
  80.     mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
  81.  
  82.     data.pointcloud = G_pcToShow;
  83.     data.image = G_imToShow;
  84.     data.pos = Tcw;
  85.     return data;
  86. }
        c.修改CMakeList.txt

        需要再其中添加PCL库，在CMakeList中添加如下三行：

  1. ...
  2. find_package(PCL REQUIRED)    # 添加行
  3.  
  4. include_directories(
  5. ${PROJECT_SOURCE_DIR}
  6. ${PROJECT_SOURCE_DIR}/include
  7. ${PROJECT_SOURCE_DIR}/include/CameraModels
  8. ${PROJECT_SOURCE_DIR}/Thirdparty/Sophus
  9. ${EIGEN3_INCLUDE_DIR}
  10. ${Pangolin_INCLUDE_DIRS}
  11. ${PCL_INCLUDE_DIRS}            # 添加行
  12. )
  13. ...
  14. target_link_libraries(${PROJECT_NAME}
  15. ${OpenCV_LIBS}
  16. ${EIGEN3_LIBS}
  17. ${Pangolin_LIBRARIES}
  18. ${PCL_LIBRARIES}                # 添加行
  19. ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
  20. ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
  21. -lboost_serialization
  22. -lcrypto
  23. )

        完成后以上步骤后，可以重新编译ORB_SLAM3。

② 改写ROS应用层

    i. 修改ros_mono.cc

        ROS节点改写后如下：

  1. #include<iostream>
  2. #include<algorithm>
  3. #include<fstream>
  4. #include<chrono>
  5.  
  6. #include<ros/ros.h>
  7. #include<cv_bridge/cv_bridge.h>
  8. #include<opencv2/core/core.hpp>
  9. #include<sensor_msgs/PointCloud2.h>
  10. #include<sensor_msgs/Image.h>
  11. #include<std_msgs/Header.h>
  12.  
  13. #include"../../../include/System.h"
  14.  
  15. #include <pcl/point_types.h>
  16. #include <pcl/point_cloud.h>
  17. #include <pcl/io/pcd_io.h>
  18. #include <pcl_conversions/pcl_conversions.h>
  19.  
  20. using namespace std;
  21.  
  22. /***************** <WSJ> CODE ******************/
  23. #define cfg_NODE_NAME               "mono"
  24. #define cfg_ROS_IMG_INPUT_TOPIC     "/iris_0/realsense/depth_camera/color/image_raw"
  25. #define cfg_ROS_IMG_OUTPUT_TOPIC    "/orb_slam3/output/image"
  26. #define cfg_ROS_PC_OUTPUT_TOPIC     "/orb_slam3/output/pointcloud2"
  27.  
  28. #define RED         "\e[1;31m"
  29. #define YELLOW      "\e[1;33m"
  30. #define GREEN       "\e[1;32m"
  31. #define BLUE        "\e[1;34m"
  32. #define COLOR_TILE  "\e[0m"
  33.  
  34. ros::Publisher img_pub;
  35. ros::Publisher pc_pub;
  36.  
  37. sensor_msgs::ImagePtr img_msg;
  38. sensor_msgs::PointCloud2 pc_msg;
  39. /*************** <WSJ> CODE END ****************/
  40.  
  41. class ImageGrabber
  42. {
  43. public:
  44.     ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){}
  45.  
  46.     void GrabImage(const sensor_msgs::ImageConstPtr& msg);
  47.  
  48.     ORB_SLAM3::System* mpSLAM;      // 创建SLMA系统指针
  49. };
  50.  
  51. int main(int argc, char **argv)
  52. {
  53.     ros::init(argc, argv, cfg_NODE_NAME);
  54.     ros::start();
  55.  
  56.     if(argc != 3)
  57.     {
  58.         cerr << endl << "Usage: rosrun ORB_SLAM3 Mono path_to_vocabulary path_to_settings" << endl;        
  59.         ros::shutdown();
  60.         return 1;
  61.     }
  62.  
  63.     // Create SLAM system. It initializes all system threads and gets ready to process frames.
  64.     ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR,true);
  65.  
  66.     ImageGrabber igb(&SLAM);
  67.  
  68.     ros::NodeHandle nodeHandler;
  69.  
  70.     ros::Subscriber sub = nodeHandler.subscribe(cfg_ROS_IMG_INPUT_TOPIC, 1, &ImageGrabber::GrabImage, &igb);
  71.     img_pub = nodeHandler.advertise<sensor_msgs::Image>(cfg_ROS_IMG_OUTPUT_TOPIC, 10);
  72.     pc_pub = nodeHandler.advertise<sensor_msgs::PointCloud2>(cfg_ROS_PC_OUTPUT_TOPIC, 10);
  73.  
  74.     cout << GREEN << "\n\n[INFO]<ros_mono.cc>: Node Start." << COLOR_TILE << endl;
  75.     cout << "---------------------------------------------------------------------" << endl;
  76.  
  77.     ros::spin();
  78.  
  79.     // Stop all threads
  80.     SLAM.Shutdown();
  81.  
  82.     // Save camera trajectory
  83.     SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
  84.  
  85.     ros::shutdown();
  86.  
  87.     return 0;
  88. }
  89.  
  90. // ROS接收消息回调函数
  91. void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
  92. {
  93.     // Copy the ros image message to cv::Mat.
  94.     cv_bridge::CvImageConstPtr cv_ptr;
  95.     try
  96.     {
  97.         cv_ptr = cv_bridge::toCvShare(msg);
  98.     }
  99.     catch (cv_bridge::Exception& e)
  100.     {
  101.         ROS_ERROR("[ROS]<%s> cv_bridge r2c exception: %s", cfg_NODE_NAME, e.what());
  102.         return;
  103.     }
  104.  
  105.     //mpSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec());     // SLAM系统追踪单目相机 返回相机位置数据
  106.  
  107.     ORB_SLAM3::SLAM_Output data;
  108.     data = mpSLAM->TrackMonocularOutput(cv_ptr->image, cv_ptr->header.stamp.toSec());
  109.  
  110.     // 发布IMG到ROS消息
  111.     std_msgs::Header header;
  112.     header.frame_id = "camera";
  113.     header.stamp = ros::Time::now();
  114.     try
  115.     {
  116.         img_msg = cv_bridge::CvImage(header, "rgb8", data.image).toImageMsg();
  117.         img_pub.publish(*img_msg);
  118.     }
  119.     catch (cv_bridge::Exception& e)
  120.     {
  121.         ROS_ERROR("[ROS]<%s> cv_bridge c2r exception: %s", cfg_NODE_NAME, e.what());
  122.         return;
  123.     }
  124.  
  125.     // 发布PC到ROS消息
  126.     pcl::toROSMsg(*(data.pointcloud), pc_msg);
  127.     pc_msg.header = header;
  128.     pc_pub.publish(pc_msg);
  129.  
  130.     //cout << data.pos.matrix() << endl << endl;
  131.     //if((data.pointcloud)->points.size())
  132.         //pcl::io::savePCDFileBinary("orb_slam3.pcd", *(data.pointcloud));      //此行会莫名其妙导致运行<Tracking.cc>623行报错退出
  133. }

         应用层中，主要是调用了上一步已经修改好的TrackMonocularOutput函数替换原来使用的TrackMonocular函数，并将返回值通过ROS发布。

    ii. 修改CMakeList.txt

        需要在其中添加PCL库，在CMakeList中添加如下2行：

  1. ...
  2. find_package(PCL REQUIRED)    # 添加行
  3.  
  4. include_directories(
  5. ${PROJECT_SOURCE_DIR}
  6. ${PROJECT_SOURCE_DIR}/../../../
  7. ${PROJECT_SOURCE_DIR}/../../../include
  8. ${PROJECT_SOURCE_DIR}/../../../include/CameraModels
  9. ${PROJECT_SOURCE_DIR}/../../../Thirdparty/Sophus
  10. ${Pangolin_INCLUDE_DIRS}
  11. ${PCL_INCLUDE_DIRS}            # 添加行
  12. )

一些参考资料：

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

  1. <?xml version="1.0" ?>
  2. <sdf version='1.5'>
  3.   <model name='iris_all_sensor'>
  4.     <!--iris body-->
  5.     <link name='base_link'>
  6.       <pose frame=''>0 0 0 0 -0 0</pose>
  7.       <inertial>
  8.         <pose frame=''>0 0 0 0 -0 0</pose>
  9.         <mass>1.5</mass>
  10.         <inertia>
  11.           <ixx>0.029125</ixx>
  12.           <ixy>0</ixy>
  13.           <ixz>0</ixz>
  14.           <iyy>0.029125</iyy>
  15.           <iyz>0</iyz>
  16.           <izz>0.055225</izz>
  17.         </inertia>
  18.       </inertial>
  19.       <collision name='base_link_inertia_collision'>
  20.         <pose frame=''>0 0 0 0 -0 0</pose>
  21.         <geometry>
  22.           <box>
  23.             <size>0.47 0.47 0.11</size>
  24.           </box>
  25.         </geometry>
  26.         <surface>
  27.           <contact>
  28.             <ode>
  29.               <min_depth>0.001</min_depth>
  30.               <max_vel>0</max_vel>
  31.             </ode>
  32.           </contact>
  33.           <friction>
  34.             <ode/>
  35.           </friction>
  36.         </surface>
  37.       </collision>
  38.       <visual name='base_link_inertia_visual'>
  39.         <pose frame=''>0 0 0 0 -0 0</pose>
  40.         <geometry>
  41.           <mesh>
  42.             <scale>1 1 1</scale>
  43.             <uri>model://rotors_description/meshes/iris.stl</uri>
  44.           </mesh>
  45.         </geometry>
  46.         <material>
  47.           <script>
  48.             <name>Gazebo/DarkGrey</name>
  49.             <uri>file://media/materials/scripts/gazebo.material</uri>
  50.           </script>
  51.         </material>
  52.       </visual>
  53.       <gravity>1</gravity>
  54.       <velocity_decay/>
  55.     </link>
  56.  
  57.  
  58.     <link name='/imu_link'>
  59.       <pose frame=''>0 0 0 0 -0 0</pose>
  60.       <inertial>
  61.         <pose frame=''>0 0 0 0 -0 0</pose>
  62.         <mass>0.015</mass>
  63.         <inertia>
  64.           <ixx>1e-05</ixx>
  65.           <ixy>0</ixy>
  66.           <ixz>0</ixz>
  67.           <iyy>1e-05</iyy>
  68.           <iyz>0</iyz>
  69.           <izz>1e-05</izz>
  70.         </inertia>
  71.       </inertial>
  72.     </link>
  73.     <joint name='/imu_joint' type='revolute'>
  74.       <child>/imu_link</child>
  75.       <parent>base_link</parent>
  76.       <axis>
  77.         <xyz>1 0 0</xyz>
  78.         <limit>
  79.           <lower>0</lower>
  80.           <upper>0</upper>
  81.           <effort>0</effort>
  82.           <velocity>0</velocity>
  83.         </limit>
  84.         <dynamics>
  85.           <spring_reference>0</spring_reference>
  86.           <spring_stiffness>0</spring_stiffness>
  87.         </dynamics>
  88.         <use_parent_model_frame>1</use_parent_model_frame>
  89.       </axis>
  90.     </joint>
  91.  
  92.     <link name='rotor_0'>
  93.       <pose frame=''>0.13 -0.22 0.023 0 -0 0</pose>
  94.       <inertial>
  95.         <pose frame=''>0 0 0 0 -0 0</pose>
  96.         <mass>0.005</mass>
  97.         <inertia>
  98.           <ixx>9.75e-07</ixx>
  99.           <ixy>0</ixy>
  100.           <ixz>0</ixz>
  101.           <iyy>0.000273104</iyy>
  102.           <iyz>0</iyz>
  103.           <izz>0.000274004</izz>
  104.         </inertia>
  105.       </inertial>
  106.       <collision name='rotor_0_collision'>
  107.         <pose frame=''>0 0 0 0 -0 0</pose>
  108.         <geometry>
  109.           <cylinder>
  110.             <length>0.005</length>
  111.             <radius>0.128</radius>
  112.           </cylinder>
  113.         </geometry>
  114.         <surface>
  115.           <contact>
  116.             <ode/>
  117.           </contact>
  118.           <friction>
  119.             <ode/>
  120.           </friction>
  121.         </surface>
  122.       </collision>
  123.       <visual name='rotor_0_visual'>
  124.         <pose frame=''>0 0 0 0 -0 0</pose>
  125.         <geometry>
  126.           <mesh>
  127.             <scale>1 1 1</scale>
  128.             <uri>model://rotors_description/meshes/iris_prop_ccw.dae</uri>
  129.           </mesh>
  130.         </geometry>
  131.         <material>
  132.           <script>
  133.             <name>Gazebo/Blue</name>
  134.             <uri>file://media/materials/scripts/gazebo.material</uri>
  135.           </script>
  136.         </material>
  137.       </visual>
  138.       <gravity>1</gravity>
  139.       <velocity_decay/>
  140.     </link>
  141.     <joint name='rotor_0_joint' type='revolute'>
  142.       <child>rotor_0</child>
  143.       <parent>base_link</parent>
  144.       <axis>
  145.         <xyz>0 0 1</xyz>
  146.         <limit>
  147.           <lower>-1e+16</lower>
  148.           <upper>1e+16</upper>
  149.         </limit>
  150.         <dynamics>
  151.           <spring_reference>0</spring_reference>
  152.           <spring_stiffness>0</spring_stiffness>
  153.         </dynamics>
  154.         <use_parent_model_frame>1</use_parent_model_frame>
  155.       </axis>
  156.     </joint>
  157.     <link name='rotor_1'>
  158.       <pose frame=''>-0.13 0.2 0.023 0 -0 0</pose>
  159.       <inertial>
  160.         <pose frame=''>0 0 0 0 -0 0</pose>
  161.         <mass>0.005</mass>
  162.         <inertia>
  163.           <ixx>9.75e-07</ixx>
  164.           <ixy>0</ixy>
  165.           <ixz>0</ixz>
  166.           <iyy>0.000273104</iyy>
  167.           <iyz>0</iyz>
  168.           <izz>0.000274004</izz>
  169.         </inertia>
  170.       </inertial>
  171.       <collision name='rotor_1_collision'>
  172.         <pose frame=''>0 0 0 0 -0 0</pose>
  173.         <geometry>
  174.           <cylinder>
  175.             <length>0.005</length>
  176.             <radius>0.128</radius>
  177.           </cylinder>
  178.         </geometry>
  179.         <surface>
  180.           <contact>
  181.             <ode/>
  182.           </contact>
  183.           <friction>
  184.             <ode/>
  185.           </friction>
  186.         </surface>
  187.       </collision>
  188.       <visual name='rotor_1_visual'>
  189.         <pose frame=''>0 0 0 0 -0 0</pose>
  190.         <geometry>
  191.           <mesh>
  192.             <scale>1 1 1</scale>
  193.             <uri>model://rotors_description/meshes/iris_prop_ccw.dae</uri>
  194.           </mesh>
  195.         </geometry>
  196.         <material>
  197.           <script>
  198.             <name>Gazebo/DarkGrey</name>
  199.             <uri>file://media/materials/scripts/gazebo.material</uri>
  200.           </script>
  201.         </material>
  202.       </visual>
  203.       <gravity>1</gravity>
  204.       <velocity_decay/>
  205.     </link>
  206.     <joint name='rotor_1_joint' type='revolute'>
  207.       <child>rotor_1</child>
  208.       <parent>base_link</parent>
  209.       <axis>
  210.         <xyz>0 0 1</xyz>
  211.         <limit>
  212.           <lower>-1e+16</lower>
  213.           <upper>1e+16</upper>
  214.         </limit>
  215.         <dynamics>
  216.           <spring_reference>0</spring_reference>
  217.           <spring_stiffness>0</spring_stiffness>
  218.         </dynamics>
  219.         <use_parent_model_frame>1</use_parent_model_frame>
  220.       </axis>
  221.     </joint>
  222.     <link name='rotor_2'>
  223.       <pose frame=''>0.13 0.22 0.023 0 -0 0</pose>
  224.       <inertial>
  225.         <pose frame=''>0 0 0 0 -0 0</pose>
  226.         <mass>0.005</mass>
  227.         <inertia>
  228.           <ixx>9.75e-07</ixx>
  229.           <ixy>0</ixy>
  230.           <ixz>0</ixz>
  231.           <iyy>0.000273104</iyy>
  232.           <iyz>0</iyz>
  233.           <izz>0.000274004</izz>
  234.         </inertia>
  235.       </inertial>
  236.       <collision name='rotor_2_collision'>
  237.         <pose frame=''>0 0 0 0 -0 0</pose>
  238.         <geometry>
  239.           <cylinder>
  240.             <length>0.005</length>
  241.             <radius>0.128</radius>
  242.           </cylinder>
  243.         </geometry>
  244.         <surface>
  245.           <contact>
  246.             <ode/>
  247.           </contact>
  248.           <friction>
  249.             <ode/>
  250.           </friction>
  251.         </surface>
  252.       </collision>
  253.       <visual name='rotor_2_visual'>
  254.         <pose frame=''>0 0 0 0 -0 0</pose>
  255.         <geometry>
  256.           <mesh>
  257.             <scale>1 1 1</scale>
  258.             <uri>model://rotors_description/meshes/iris_prop_cw.dae</uri>
  259.           </mesh>
  260.         </geometry>
  261.         <material>
  262.           <script>
  263.             <name>Gazebo/Blue</name>
  264.             <uri>file://media/materials/scripts/gazebo.material</uri>
  265.           </script>
  266.         </material>
  267.       </visual>
  268.       <gravity>1</gravity>
  269.       <velocity_decay/>
  270.     </link>
  271.     <joint name='rotor_2_joint' type='revolute'>
  272.       <child>rotor_2</child>
  273.       <parent>base_link</parent>
  274.       <axis>
  275.         <xyz>0 0 1</xyz>
  276.         <limit>
  277.           <lower>-1e+16</lower>
  278.           <upper>1e+16</upper>
  279.         </limit>
  280.         <dynamics>
  281.           <spring_reference>0</spring_reference>
  282.           <spring_stiffness>0</spring_stiffness>
  283.         </dynamics>
  284.         <use_parent_model_frame>1</use_parent_model_frame>
  285.       </axis>
  286.     </joint>
  287.     <link name='rotor_3'>
  288.       <pose frame=''>-0.13 -0.2 0.023 0 -0 0</pose>
  289.       <inertial>
  290.         <pose frame=''>0 0 0 0 -0 0</pose>
  291.         <mass>0.005</mass>
  292.         <inertia>
  293.           <ixx>9.75e-07</ixx>
  294.           <ixy>0</ixy>
  295.           <ixz>0</ixz>
  296.           <iyy>0.000273104</iyy>
  297.           <iyz>0</iyz>
  298.           <izz>0.000274004</izz>
  299.         </inertia>
  300.       </inertial>
  301.       <collision name='rotor_3_collision'>
  302.         <pose frame=''>0 0 0 0 -0 0</pose>
  303.         <geometry>
  304.           <cylinder>
  305.             <length>0.005</length>
  306.             <radius>0.128</radius>
  307.           </cylinder>
  308.         </geometry>
  309.         <surface>
  310.           <contact>
  311.             <ode/>
  312.           </contact>
  313.           <friction>
  314.             <ode/>
  315.           </friction>
  316.         </surface>
  317.       </collision>
  318.       <visual name='rotor_3_visual'>
  319.         <pose frame=''>0 0 0 0 -0 0</pose>
  320.         <geometry>
  321.           <mesh>
  322.             <scale>1 1 1</scale>
  323.             <uri>model://rotors_description/meshes/iris_prop_cw.dae</uri>
  324.           </mesh>
  325.         </geometry>
  326.         <material>
  327.           <script>
  328.             <name>Gazebo/DarkGrey</name>
  329.             <uri>file://media/materials/scripts/gazebo.material</uri>
  330.           </script>
  331.         </material>
  332.       </visual>
  333.       <gravity>1</gravity>
  334.       <velocity_decay/>
  335.     </link>
  336.     <joint name='rotor_3_joint' type='revolute'>
  337.       <child>rotor_3</child>
  338.       <parent>base_link</parent>
  339.       <axis>
  340.         <xyz>0 0 1</xyz>
  341.         <limit>
  342.           <lower>-1e+16</lower>
  343.           <upper>1e+16</upper>
  344.         </limit>
  345.         <dynamics>
  346.           <spring_reference>0</spring_reference>
  347.           <spring_stiffness>0</spring_stiffness>
  348.         </dynamics>
  349.         <use_parent_model_frame>1</use_parent_model_frame>
  350.       </axis>
  351.     </joint>
  352.  
  353.  
  354.  
  355.  
  356.     <plugin name='rosbag' filename='libgazebo_multirotor_base_plugin.so'>
  357.       <robotNamespace/>
  358.       <linkName>base_link</linkName>
  359.       <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
  360.     </plugin>
  361.     <plugin name='front_right_motor_model' filename='libgazebo_motor_model.so'>
  362.       <robotNamespace/>
  363.       <jointName>rotor_0_joint</jointName>
  364.       <linkName>rotor_0</linkName>
  365.       <turningDirection>ccw</turningDirection>
  366.       <timeConstantUp>0.0125</timeConstantUp>
  367.       <timeConstantDown>0.025</timeConstantDown>
  368.       <maxRotVelocity>1100</maxRotVelocity>
  369.       <motorConstant>8.54858e-06</motorConstant>
  370.       <momentConstant>0.06</momentConstant>
  371.       <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
  372.       <motorNumber>0</motorNumber>
  373.       <rotorDragCoefficient>0.000175</rotorDragCoefficient>
  374.       <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
  375.       <motorSpeedPubTopic>/motor_speed/0</motorSpeedPubTopic>
  376.       <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
  377.     </plugin>
  378.     <plugin name='back_left_motor_model' filename='libgazebo_motor_model.so'>
  379.       <robotNamespace/>
  380.       <jointName>rotor_1_joint</jointName>
  381.       <linkName>rotor_1</linkName>
  382.       <turningDirection>ccw</turningDirection>
  383.       <timeConstantUp>0.0125</timeConstantUp>
  384.       <timeConstantDown>0.025</timeConstantDown>
  385.       <maxRotVelocity>1100</maxRotVelocity>
  386.       <motorConstant>8.54858e-06</motorConstant>
  387.       <momentConstant>0.06</momentConstant>
  388.       <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
  389.       <motorNumber>1</motorNumber>
  390.       <rotorDragCoefficient>0.000175</rotorDragCoefficient>
  391.       <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
  392.       <motorSpeedPubTopic>/motor_speed/1</motorSpeedPubTopic>
  393.       <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
  394.     </plugin>
  395.     <plugin name='front_left_motor_model' filename='libgazebo_motor_model.so'>
  396.       <robotNamespace/>
  397.       <jointName>rotor_2_joint</jointName>
  398.       <linkName>rotor_2</linkName>
  399.       <turningDirection>cw</turningDirection>
  400.       <timeConstantUp>0.0125</timeConstantUp>
  401.       <timeConstantDown>0.025</timeConstantDown>
  402.       <maxRotVelocity>1100</maxRotVelocity>
  403.       <motorConstant>8.54858e-06</motorConstant>
  404.       <momentConstant>0.06</momentConstant>
  405.       <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
  406.       <motorNumber>2</motorNumber>
  407.       <rotorDragCoefficient>0.000175</rotorDragCoefficient>
  408.       <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
  409.       <motorSpeedPubTopic>/motor_speed/2</motorSpeedPubTopic>
  410.       <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
  411.     </plugin>
  412.     <plugin name='back_right_motor_model' filename='libgazebo_motor_model.so'>
  413.       <robotNamespace/>
  414.       <jointName>rotor_3_joint</jointName>
  415.       <linkName>rotor_3</linkName>
  416.       <turningDirection>cw</turningDirection>
  417.       <timeConstantUp>0.0125</timeConstantUp>
  418.       <timeConstantDown>0.025</timeConstantDown>
  419.       <maxRotVelocity>1100</maxRotVelocity>
  420.       <motorConstant>8.54858e-06</motorConstant>
  421.       <momentConstant>0.06</momentConstant>
  422.       <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
  423.       <motorNumber>3</motorNumber>
  424.       <rotorDragCoefficient>0.000175</rotorDragCoefficient>
  425.       <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
  426.       <motorSpeedPubTopic>/motor_speed/3</motorSpeedPubTopic>
  427.       <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
  428.     </plugin>
  429.     <plugin name='gps_plugin' filename='libgazebo_gps_plugin.so'>
  430.       <robotNamespace/>
  431.       <gpsNoise>1</gpsNoise>
  432.     </plugin>
  433.     <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
  434.       <robotNamespace/>
  435.       <pubRate>100</pubRate>
  436.       <noiseDensity>0.0004</noiseDensity>
  437.       <randomWalk>6.4e-06</randomWalk>
  438.       <biasCorrelationTime>600</biasCorrelationTime>
  439.       <magTopic>/mag</magTopic>
  440.     </plugin>
  441.     <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
  442.       <robotNamespace/>
  443.       <pubRate>50</pubRate>
  444.       <baroTopic>/baro</baroTopic>
  445.     </plugin>
  446.     <plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
  447.       <robotNamespace/>
  448.       <imuSubTopic>/imu</imuSubTopic>
  449.       <gpsSubTopic>/gps</gpsSubTopic>
  450.       <magSubTopic>/mag</magSubTopic>
  451.       <baroSubTopic>/baro</baroSubTopic>
  452.       <mavlink_addr>INADDR_ANY</mavlink_addr>
  453.       <mavlink_udp_port>14560</mavlink_udp_port>
  454.       <mavlink_tcp_port>4560</mavlink_tcp_port>
  455.       <serialEnabled>0</serialEnabled>
  456.       <serialDevice>/dev/ttyACM0</serialDevice>
  457.       <baudRate>921600</baudRate>
  458.       <qgc_addr>INADDR_ANY</qgc_addr>
  459.       <qgc_udp_port>14550</qgc_udp_port>
  460.       <sdk_addr>INADDR_ANY</sdk_addr>
  461.       <sdk_udp_port>14540</sdk_udp_port>
  462.       <hil_mode>0</hil_mode>
  463.       <hil_state_level>0</hil_state_level>
  464.       <vehicle_is_tailsitter>0</vehicle_is_tailsitter>
  465.       <send_vision_estimation>1</send_vision_estimation>
  466.       <send_odometry>0</send_odometry>
  467.       <enable_lockstep>1</enable_lockstep>
  468.       <use_tcp>1</use_tcp>
  469.       <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
  470.       <control_channels>
  471.         <channel name='rotor1'>
  472.           <input_index>0</input_index>
  473.           <input_offset>0</input_offset>
  474.           <input_scaling>1000</input_scaling>
  475.           <zero_position_disarmed>0</zero_position_disarmed>
  476.           <zero_position_armed>100</zero_position_armed>
  477.           <joint_control_type>velocity</joint_control_type>
  478.         </channel>
  479.         <channel name='rotor2'>
  480.           <input_index>1</input_index>
  481.           <input_offset>0</input_offset>
  482.           <input_scaling>1000</input_scaling>
  483.           <zero_position_disarmed>0</zero_position_disarmed>
  484.           <zero_position_armed>100</zero_position_armed>
  485.           <joint_control_type>velocity</joint_control_type>
  486.         </channel>
  487.         <channel name='rotor3'>
  488.           <input_index>2</input_index>
  489.           <input_offset>0</input_offset>
  490.           <input_scaling>1000</input_scaling>
  491.           <zero_position_disarmed>0</zero_position_disarmed>
  492.           <zero_position_armed>100</zero_position_armed>
  493.           <joint_control_type>velocity</joint_control_type>
  494.         </channel>
  495.         <channel name='rotor4'>
  496.           <input_index>3</input_index>
  497.           <input_offset>0</input_offset>
  498.           <input_scaling>1000</input_scaling>
  499.           <zero_position_disarmed>0</zero_position_disarmed>
  500.           <zero_position_armed>100</zero_position_armed>
  501.           <joint_control_type>velocity</joint_control_type>
  502.         </channel>
  503.         <channel name='rotor5'>
  504.           <input_index>4</input_index>
  505.           <input_offset>1</input_offset>
  506.           <input_scaling>324.6</input_scaling>
  507.           <zero_position_disarmed>0</zero_position_disarmed>
  508.           <zero_position_armed>0</zero_position_armed>
  509.           <joint_control_type>velocity</joint_control_type>
  510.           <joint_control_pid>
  511.             <p>0.1</p>
  512.             <i>0</i>
  513.             <d>0</d>
  514.             <iMax>0.0</iMax>
  515.             <iMin>0.0</iMin>
  516.             <cmdMax>2</cmdMax>
  517.             <cmdMin>-2</cmdMin>
  518.           </joint_control_pid>
  519.           <joint_name>zephyr_delta_wing::propeller_joint</joint_name>
  520.         </channel>
  521.         <channel name='rotor6'>
  522.           <input_index>5</input_index>
  523.           <input_offset>0</input_offset>
  524.           <input_scaling>0.524</input_scaling>
  525.           <zero_position_disarmed>0</zero_position_disarmed>
  526.           <zero_position_armed>0</zero_position_armed>
  527.           <joint_control_type>position</joint_control_type>
  528.           <joint_name>zephyr_delta_wing::flap_left_joint</joint_name>
  529.           <joint_control_pid>
  530.             <p>10.0</p>
  531.             <i>0</i>
  532.             <d>0</d>
  533.             <iMax>0</iMax>
  534.             <iMin>0</iMin>
  535.             <cmdMax>20</cmdMax>
  536.             <cmdMin>-20</cmdMin>
  537.           </joint_control_pid>
  538.         </channel>
  539.         <channel name='rotor7'>
  540.           <input_index>6</input_index>
  541.           <input_offset>0</input_offset>
  542.           <input_scaling>0.524</input_scaling>
  543.           <zero_position_disarmed>0</zero_position_disarmed>
  544.           <zero_position_armed>0</zero_position_armed>
  545.           <joint_control_type>position</joint_control_type>
  546.           <joint_name>zephyr_delta_wing::flap_right_joint</joint_name>
  547.           <joint_control_pid>
  548.             <p>10.0</p>
  549.             <i>0</i>
  550.             <d>0</d>
  551.             <iMax>0</iMax>
  552.             <iMin>0</iMin>
  553.             <cmdMax>20</cmdMax>
  554.             <cmdMin>-20</cmdMin>
  555.           </joint_control_pid>
  556.         </channel>
  557.         <channel name='rotor8'>
  558.           <input_index>7</input_index>
  559.           <input_offset>0</input_offset>
  560.           <input_scaling>0.524</input_scaling>
  561.           <zero_position_disarmed>0</zero_position_disarmed>
  562.           <zero_position_armed>0</zero_position_armed>
  563.           <joint_control_type>position</joint_control_type>
  564.         </channel>
  565.       </control_channels>
  566.     </plugin>
  567.     <static>0</static>
  568.     <plugin name='rotors_gazebo_imu_plugin' filename='libgazebo_imu_plugin.so'>
  569.       <robotNamespace/>
  570.       <linkName>/imu_link</linkName>
  571.       <imuTopic>/imu</imuTopic>
  572.       <gyroscopeNoiseDensity>0.00018665</gyroscopeNoiseDensity>
  573.       <gyroscopeRandomWalk>3.8785e-05</gyroscopeRandomWalk>
  574.       <gyroscopeBiasCorrelationTime>1000.0</gyroscopeBiasCorrelationTime>
  575.       <gyroscopeTurnOnBiasSigma>0.0087</gyroscopeTurnOnBiasSigma>
  576.       <accelerometerNoiseDensity>0.00186</accelerometerNoiseDensity>
  577.       <accelerometerRandomWalk>0.006</accelerometerRandomWalk>
  578.       <accelerometerBiasCorrelationTime>300.0</accelerometerBiasCorrelationTime>
  579.       <accelerometerTurnOnBiasSigma>0.196</accelerometerTurnOnBiasSigma>
  580.     </plugin>
  581.  
  582.     <include>
  583.       <uri>model://realsense_camera</uri>
  584.       <pose>0.1 0 0 0 0 0</pose>
  585.     </include>
  586.     <joint name="realsense_camera_joint" type="revolute">
  587.       <child>realsense_camera::link</child>
  588.       <parent>base_link</parent>
  589.       <axis>
  590.         <xyz>0 0 1</xyz>
  591.         <limit>
  592.           <upper>0</upper>
  593.           <lower>0</lower>
  594.         </limit>
  595.       </axis>
  596.     </joint>
  597.  
  598.     <include>
  599.       <uri>model://monocular_camera</uri>
  600.       <pose>0 0 -0.03 0 1.5707963 0</pose>
  601.     </include>
  602.     <joint name="monocular_down_joint" type="fixed">
  603.       <child>monocular_camera::link</child>
  604.       <parent>base_link</parent>
  605.       <axis>
  606.         <xyz>0 0 1</xyz>
  607.         <limit>
  608.           <upper>0</upper>
  609.           <lower>0</lower>
  610.         </limit>
  611.       </axis>
  612.     </joint>
  613.  
  614.     <!-- For Hokuyo Lidar Payload -->
  615.     <include>
  616.         <uri>model://hokuyo_lidar</uri>
  617.         <pose>0 0 0.06 0 0 0</pose>
  618.       </include>
  619.     <joint name="lidar_joint" type="fixed">
  620.       <child>hokuyo_lidar::link</child>
  621.       <parent>base_link</parent>
  622.       <axis>
  623.         <xyz>0 0 1</xyz>
  624.         <limit>
  625.           <upper>0</upper>
  626.           <lower>0</lower>
  627.         </limit>
  628.       </axis>
  629.     </joint>
  630.  
  631.     <!-- For laser Range Finder Payload -->
  632.     <include>
  633.         <uri>model://laser_rangefinder</uri>
  634.         <pose>0 0 -0.05 0 1.570796 0</pose>
  635.       </include>
  636.     <joint name="laser_1d_joint" type="fixed">
  637.       <child>laser_rangefinder::link</child>
  638.       <parent>base_link</parent>
  639.       <axis>
  640.         <xyz>0 0 1</xyz>
  641.         <limit>
  642.           <upper>0</upper>
  643.           <lower>0</lower>
  644.         </limit>
  645.       </axis>
  646.     </joint>
  647.  
  648.     <!-- For IMU-->
  649.     <include>
  650.       <uri>model://imu_gazebo</uri>
  651.       <pose>0 0 0.3 0 0 0</pose>
  652.     </include>
  653.     <joint name="imu_gazebo_joint" type="fixed">
  654.       <child>imu_gazebo::link</child>
  655.       <parent>base_link</parent>
  656.       <axis>
  657.         <xyz>0 0 1</xyz>
  658.         <limit>
  659.           <upper>0</upper>
  660.           <lower>0</lower>
  661.         </limit>
  662.       </axis>
  663.     </joint>
  664.   </model>
  665. </sdf>
  666. <!-- vim: set noet fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->

model.config

  1. <?xml version="1.0"?>
  2. <model>
  3.   <name>Iris with all sensor</name>
  4.   <version>1.0</version>
  5.   <sdf version='1.5'>iris_all_sensor.sdf</sdf>
  6.  
  7.   <author>
  8.    <name>wsj</name>
  9.    <email>wangshujie@nuaa.edu.cn</email>
  10.   </author>
  11.  
  12.   <description>
  13.     This is a model of the 3DR Iris Quadrotor for Zhihangcup competition.
  14.   </description>
  15. </model>

附录二：无人机测试环境及launch文件

map_maze_target.world

  1. <sdf version='1.6'>
  2.   <world name='default'>
  3.     <light name='sun' type='directional'>
  4.       <cast_shadows>1</cast_shadows>
  5.       <pose frame=''>0 0 10 0 -0 0</pose>
  6.       <diffuse>0.8 0.8 0.8 1</diffuse>
  7.       <specular>0.2 0.2 0.2 1</specular>
  8.       <attenuation>
  9.         <range>1000</range>
  10.         <constant>0.9</constant>
  11.         <linear>0.01</linear>
  12.         <quadratic>0.001</quadratic>
  13.       </attenuation>
  14.       <direction>-0.5 0.1 -0.9</direction>
  15.     </light>
  16.     <model name='ceiling'>
  17.       <link name='link'>
  18.         <inertial>
  19.           <pose frame=''>0 0 0 0 0 0</pose>
  20.           <mass>100000</mass>
  21.           <inertia>
  22.             <ixx>100000</ixx>
  23.             <ixy>0</ixy>
  24.             <ixz>0</ixz>
  25.             <iyy>100000</iyy>
  26.             <iyz>0</iyz>
  27.             <izz>100000</izz>
  28.           </inertia>
  29.         </inertial>
  30.         <collision name='collision'>
  31.           <pose frame=''>0 0 0 0 -0 0</pose>
  32.           <geometry>
  33.             <box>
  34.               <size>300 300 0.5</size>
  35.             </box>
  36.           </geometry>
  37.  
  38.             <friction>
  39.               <ode>
  40.                 <mu>1</mu>
  41.                 <mu2>1</mu2>
  42.               </ode>
  43.               <torsional>
  44.                 <ode/>
  45.               </torsional>
  46.             </friction>
  47.             <contact>
  48.               <ode>
  49.                 <kp>1e+07</kp>
  50.                 <kd>1</kd>
  51.                 <min_depth>0.001</min_depth>
  52.                 <max_vel>0.1</max_vel>
  53.               </ode>
  54.             </contact>
  55.             <bounce/>
  56.  
  57.           <max_contacts>10</max_contacts>
  58.         </collision>
  59.         <self_collide>0</self_collide>
  60.         <enable_wind>0</enable_wind>
  61.         <kinematic>0</kinematic>
  62.       </link>
  63.       <pose frame=''>0 0 5 0 0 0</pose>
  64.     </model>
  65.     <model name='ground_plane'>
  66.       <static>1</static>
  67.       <link name='link'>
  68.         <collision name='collision'>
  69.           <geometry>
  70.             <plane>
  71.               <normal>0 0 1</normal>
  72.               <size>300 300</size>
  73.             </plane>
  74.           </geometry>
  75.           <surface>
  76.             <friction>
  77.               <ode>
  78.                 <mu>100</mu>
  79.                 <mu2>50</mu2>
  80.               </ode>
  81.               <torsional>
  82.                 <ode/>
  83.               </torsional>
  84.             </friction>
  85.             <contact>
  86.               <ode/>
  87.             </contact>
  88.             <bounce/>
  89.           </surface>
  90.           <max_contacts>10</max_contacts>
  91.         </collision>
  92.         <visual name='visual'>
  93.           <cast_shadows>0</cast_shadows>
  94.           <geometry>
  95.             <plane>
  96.               <normal>0 0 1</normal>
  97.               <size>300 300</size>
  98.             </plane>
  99.           </geometry>
  100.           <material>
  101.             <script>
  102.               <uri>file://media/materials/scripts/gazebo.material</uri>
  103.               <name>Gazebo/Grey</name>
  104.             </script>
  105.           </material>
  106.         </visual>
  107.         <self_collide>0</self_collide>
  108.         <enable_wind>0</enable_wind>
  109.         <kinematic>0</kinematic>
  110.       </link>
  111.     </model>
  112.     <gravity>0 0 -9.8</gravity>
  113.     <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
  114.     <atmosphere type='adiabatic'/>
  115.     <physics name='default_physics' default='0' type='ode'>
  116.       <max_step_size>0.001</max_step_size>
  117.       <real_time_factor>1</real_time_factor>
  118.       <real_time_update_rate>1000</real_time_update_rate>
  119.     </physics>
  120.     <scene>
  121.       <ambient>0.4 0.4 0.4 1</ambient>
  122.       <background>0.7 0.7 0.7 1</background>
  123.       <shadows>1</shadows>
  124.     </scene>
  125.     <wind/>
  126.     <spherical_coordinates>
  127.       <surface_model>EARTH_WGS84</surface_model>
  128.       <latitude_deg>0</latitude_deg>
  129.       <longitude_deg>0</longitude_deg>
  130.       <elevation>0</elevation>
  131.       <heading_deg>0</heading_deg>
  132.     </spherical_coordinates>
  133.     <model name='map2_1'>
  134.       <pose frame=''>1.002 -6.58047 0 0 -0 0</pose>
  135.       <link name='Wall_0'>
  136.         <collision name='Wall_0_Collision'>
  137.           <geometry>
  138.             <box>
  139.               <size>7.75 0.2 2.5</size>
  140.             </box>
  141.           </geometry>
  142.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  143.           <max_contacts>10</max_contacts>
  144.           <surface>
  145.             <contact>
  146.               <ode/>
  147.             </contact>
  148.             <bounce/>
  149.             <friction>
  150.               <torsional>
  151.                 <ode/>
  152.               </torsional>
  153.               <ode/>
  154.             </friction>
  155.           </surface>
  156.         </collision>
  157.         <visual name='Wall_0_Visual'>
  158.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  159.           <geometry>
  160.             <box>
  161.               <size>7.75 0.2 2.5</size>
  162.             </box>
  163.           </geometry>
  164.           <material>
  165.             <script>
  166.               <uri>file://media/materials/scripts/gazebo.material</uri>
  167.               <name>Gazebo/Bricks</name>
  168.             </script>
  169.             <ambient>1 1 1 1</ambient>
  170.           </material>
  171.           <meta>
  172.             <layer>0</layer>
  173.           </meta>
  174.         </visual>
  175.         <pose frame=''>-18.6196 5.51467 0 0 -0 1.5708</pose>
  176.         <self_collide>0</self_collide>
  177.         <enable_wind>0</enable_wind>
  178.         <kinematic>0</kinematic>
  179.       </link>
  180.       <link name='Wall_1'>
  181.         <collision name='Wall_1_Collision'>
  182.           <geometry>
  183.             <box>
  184.               <size>37.3896 0.2 2.5</size>
  185.             </box>
  186.           </geometry>
  187.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  188.           <max_contacts>10</max_contacts>
  189.           <surface>
  190.             <contact>
  191.               <ode/>
  192.             </contact>
  193.             <bounce/>
  194.             <friction>
  195.               <torsional>
  196.                 <ode/>
  197.               </torsional>
  198.               <ode/>
  199.             </friction>
  200.           </surface>
  201.         </collision>
  202.         <visual name='Wall_1_Visual'>
  203.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  204.           <geometry>
  205.             <box>
  206.               <size>37.3896 0.2 2.5</size>
  207.             </box>
  208.           </geometry>
  209.           <material>
  210.             <script>
  211.               <uri>file://media/materials/scripts/gazebo.material</uri>
  212.               <name>Gazebo/Bricks</name>
  213.             </script>
  214.             <ambient>1 1 1 1</ambient>
  215.           </material>
  216.           <meta>
  217.             <layer>0</layer>
  218.           </meta>
  219.         </visual>
  220.         <pose frame=''>0.0002 9.31467 0 0 -0 0</pose>
  221.         <self_collide>0</self_collide>
  222.         <enable_wind>0</enable_wind>
  223.         <kinematic>0</kinematic>
  224.       </link>
  225.       <link name='Wall_10'>
  226.         <collision name='Wall_10_Collision'>
  227.           <geometry>
  228.             <box>
  229.               <size>3.93638 0.2 2.5</size>
  230.             </box>
  231.           </geometry>
  232.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  233.           <max_contacts>10</max_contacts>
  234.           <surface>
  235.             <contact>
  236.               <ode/>
  237.             </contact>
  238.             <bounce/>
  239.             <friction>
  240.               <torsional>
  241.                 <ode/>
  242.               </torsional>
  243.               <ode/>
  244.             </friction>
  245.           </surface>
  246.         </collision>
  247.         <visual name='Wall_10_Visual'>
  248.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  249.           <geometry>
  250.             <box>
  251.               <size>3.93638 0.2 2.5</size>
  252.             </box>
  253.           </geometry>
  254.           <material>
  255.             <script>
  256.               <uri>file://media/materials/scripts/gazebo.material</uri>
  257.               <name>Gazebo/Bricks</name>
  258.             </script>
  259.             <ambient>1 1 1 1</ambient>
  260.           </material>
  261.           <meta>
  262.             <layer>0</layer>
  263.           </meta>
  264.         </visual>
  265.         <pose frame=''>-13.0365 1.84322 0 0 -0 -0.012287</pose>
  266.         <self_collide>0</self_collide>
  267.         <enable_wind>0</enable_wind>
  268.         <kinematic>0</kinematic>
  269.       </link>
  270.       <link name='Wall_11'>
  271.         <collision name='Wall_11_Collision'>
  272.           <geometry>
  273.             <box>
  274.               <size>3.84334 0.2 2.5</size>
  275.             </box>
  276.           </geometry>
  277.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  278.           <max_contacts>10</max_contacts>
  279.           <surface>
  280.             <contact>
  281.               <ode/>
  282.             </contact>
  283.             <bounce/>
  284.             <friction>
  285.               <torsional>
  286.                 <ode/>
  287.               </torsional>
  288.               <ode/>
  289.             </friction>
  290.           </surface>
  291.         </collision>
  292.         <visual name='Wall_11_Visual'>
  293.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  294.           <geometry>
  295.             <box>
  296.               <size>3.84334 0.2 2.5</size>
  297.             </box>
  298.           </geometry>
  299.           <material>
  300.             <script>
  301.               <uri>file://media/materials/scripts/gazebo.material</uri>
  302.               <name>Gazebo/Bricks</name>
  303.             </script>
  304.             <ambient>1 1 1 1</ambient>
  305.           </material>
  306.           <meta>
  307.             <layer>0</layer>
  308.           </meta>
  309.         </visual>
  310.         <pose frame=''>-11.1667 -0.073086 0 0 -0 -1.58339</pose>
  311.         <self_collide>0</self_collide>
  312.         <enable_wind>0</enable_wind>
  313.         <kinematic>0</kinematic>
  314.       </link>
  315.       <link name='Wall_12'>
  316.         <collision name='Wall_12_Collision'>
  317.           <geometry>
  318.             <box>
  319.               <size>11.25 0.2 2.5</size>
  320.             </box>
  321.           </geometry>
  322.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  323.           <max_contacts>10</max_contacts>
  324.           <surface>
  325.             <contact>
  326.               <ode/>
  327.             </contact>
  328.             <bounce/>
  329.             <friction>
  330.               <torsional>
  331.                 <ode/>
  332.               </torsional>
  333.               <ode/>
  334.             </friction>
  335.           </surface>
  336.         </collision>
  337.         <visual name='Wall_12_Visual'>
  338.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  339.           <geometry>
  340.             <box>
  341.               <size>11.25 0.2 2.5</size>
  342.             </box>
  343.           </geometry>
  344.           <material>
  345.             <script>
  346.               <uri>file://media/materials/scripts/gazebo.material</uri>
  347.               <name>Gazebo/Bricks</name>
  348.             </script>
  349.             <ambient>1 1 1 1</ambient>
  350.           </material>
  351.           <meta>
  352.             <layer>0</layer>
  353.           </meta>
  354.         </visual>
  355.         <pose frame=''>-5.63996 -1.96613 0 0 -0 0</pose>
  356.         <self_collide>0</self_collide>
  357.         <enable_wind>0</enable_wind>
  358.         <kinematic>0</kinematic>
  359.       </link>
  360.       <link name='Wall_14'>
  361.         <collision name='Wall_14_Collision'>
  362.           <geometry>
  363.             <box>
  364.               <size>4 0.2 2.5</size>
  365.             </box>
  366.           </geometry>
  367.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  368.           <max_contacts>10</max_contacts>
  369.           <surface>
  370.             <contact>
  371.               <ode/>
  372.             </contact>
  373.             <bounce/>
  374.             <friction>
  375.               <torsional>
  376.                 <ode/>
  377.               </torsional>
  378.               <ode/>
  379.             </friction>
  380.           </surface>
  381.         </collision>
  382.         <visual name='Wall_14_Visual'>
  383.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  384.           <geometry>
  385.             <box>
  386.               <size>4 0.2 2.5</size>
  387.             </box>
  388.           </geometry>
  389.           <material>
  390.             <script>
  391.               <uri>file://media/materials/scripts/gazebo.material</uri>
  392.               <name>Gazebo/Bricks</name>
  393.             </script>
  394.             <ambient>1 1 1 1</ambient>
  395.           </material>
  396.           <meta>
  397.             <layer>0</layer>
  398.           </meta>
  399.         </visual>
  400.         <pose frame=''>-0.08996 -0.041134 0 0 -0 1.5708</pose>
  401.         <self_collide>0</self_collide>
  402.         <enable_wind>0</enable_wind>
  403.         <kinematic>0</kinematic>
  404.       </link>
  405.       <link name='Wall_15'>
  406.         <collision name='Wall_15_Collision'>
  407.           <geometry>
  408.             <box>
  409.               <size>4 0.2 2.5</size>
  410.             </box>
  411.           </geometry>
  412.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  413.           <max_contacts>10</max_contacts>
  414.           <surface>
  415.             <contact>
  416.               <ode/>
  417.             </contact>
  418.             <bounce/>
  419.             <friction>
  420.               <torsional>
  421.                 <ode/>
  422.               </torsional>
  423.               <ode/>
  424.             </friction>
  425.           </surface>
  426.         </collision>
  427.         <visual name='Wall_15_Visual'>
  428.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  429.           <geometry>
  430.             <box>
  431.               <size>4 0.2 2.5</size>
  432.             </box>
  433.           </geometry>
  434.           <material>
  435.             <script>
  436.               <uri>file://media/materials/scripts/gazebo.material</uri>
  437.               <name>Gazebo/Bricks</name>
  438.             </script>
  439.             <ambient>1 1 1 1</ambient>
  440.           </material>
  441.           <meta>
  442.             <layer>0</layer>
  443.           </meta>
  444.         </visual>
  445.         <pose frame=''>1.83504 1.88387 0 0 -0 0</pose>
  446.         <self_collide>0</self_collide>
  447.         <enable_wind>0</enable_wind>
  448.         <kinematic>0</kinematic>
  449.       </link>
  450.       <link name='Wall_17'>
  451.         <collision name='Wall_17_Collision'>
  452.           <geometry>
  453.             <box>
  454.               <size>3.75 0.2 2.5</size>
  455.             </box>
  456.           </geometry>
  457.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  458.           <max_contacts>10</max_contacts>
  459.           <surface>
  460.             <contact>
  461.               <ode/>
  462.             </contact>
  463.             <bounce/>
  464.             <friction>
  465.               <torsional>
  466.                 <ode/>
  467.               </torsional>
  468.               <ode/>
  469.             </friction>
  470.           </surface>
  471.         </collision>
  472.         <visual name='Wall_17_Visual'>
  473.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  474.           <geometry>
  475.             <box>
  476.               <size>3.75 0.2 2.5</size>
  477.             </box>
  478.           </geometry>
  479.           <material>
  480.             <script>
  481.               <uri>file://media/materials/scripts/gazebo.material</uri>
  482.               <name>Gazebo/Bricks</name>
  483.             </script>
  484.             <ambient>1 1 1 1</ambient>
  485.           </material>
  486.           <meta>
  487.             <layer>0</layer>
  488.           </meta>
  489.         </visual>
  490.         <pose frame=''>-7.48571 -3.74848 0 0 -0 -1.5708</pose>
  491.         <self_collide>0</self_collide>
  492.         <enable_wind>0</enable_wind>
  493.         <kinematic>0</kinematic>
  494.       </link>
  495.       <link name='Wall_19'>
  496.         <collision name='Wall_19_Collision'>
  497.           <geometry>
  498.             <box>
  499.               <size>3.84422 0.2 2.5</size>
  500.             </box>
  501.           </geometry>
  502.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  503.           <max_contacts>10</max_contacts>
  504.           <surface>
  505.             <contact>
  506.               <ode/>
  507.             </contact>
  508.             <bounce/>
  509.             <friction>
  510.               <torsional>
  511.                 <ode/>
  512.               </torsional>
  513.               <ode/>
  514.             </friction>
  515.           </surface>
  516.         </collision>
  517.         <visual name='Wall_19_Visual'>
  518.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  519.           <geometry>
  520.             <box>
  521.               <size>3.84422 0.2 2.5</size>
  522.             </box>
  523.           </geometry>
  524.           <material>
  525.             <script>
  526.               <uri>file://media/materials/scripts/gazebo.material</uri>
  527.               <name>Gazebo/Bricks</name>
  528.             </script>
  529.             <ambient>1 1 1 1</ambient>
  530.           </material>
  531.           <meta>
  532.             <layer>0</layer>
  533.           </meta>
  534.         </visual>
  535.         <pose frame=''>5.64847 5.54188 0 0 -0 -3.1164</pose>
  536.         <self_collide>0</self_collide>
  537.         <enable_wind>0</enable_wind>
  538.         <kinematic>0</kinematic>
  539.       </link>
  540.       <link name='Wall_2'>
  541.         <collision name='Wall_2_Collision'>
  542.           <geometry>
  543.             <box>
  544.               <size>18.7327 0.2 2.5</size>
  545.             </box>
  546.           </geometry>
  547.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  548.           <max_contacts>10</max_contacts>
  549.           <surface>
  550.             <contact>
  551.               <ode/>
  552.             </contact>
  553.             <bounce/>
  554.             <friction>
  555.               <torsional>
  556.                 <ode/>
  557.               </torsional>
  558.               <ode/>
  559.             </friction>
  560.           </surface>
  561.         </collision>
  562.         <visual name='Wall_2_Visual'>
  563.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  564.           <geometry>
  565.             <box>
  566.               <size>18.7327 0.2 2.5</size>
  567.             </box>
  568.           </geometry>
  569.           <material>
  570.             <script>
  571.               <uri>file://media/materials/scripts/gazebo.material</uri>
  572.               <name>Gazebo/Bricks</name>
  573.             </script>
  574.             <ambient>1 1 1 1</ambient>
  575.           </material>
  576.           <meta>
  577.             <layer>0</layer>
  578.           </meta>
  579.         </visual>
  580.         <pose frame=''>18.5502 -0.023168 0 0 -0 -1.5733</pose>
  581.         <self_collide>0</self_collide>
  582.         <enable_wind>0</enable_wind>
  583.         <kinematic>0</kinematic>
  584.       </link>
  585.       <link name='Wall_20'>
  586.         <collision name='Wall_20_Collision'>
  587.           <geometry>
  588.             <box>
  589.               <size>7.5 0.2 2.5</size>
  590.             </box>
  591.           </geometry>
  592.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  593.           <max_contacts>10</max_contacts>
  594.           <surface>
  595.             <contact>
  596.               <ode/>
  597.             </contact>
  598.             <bounce/>
  599.             <friction>
  600.               <torsional>
  601.                 <ode/>
  602.               </torsional>
  603.               <ode/>
  604.             </friction>
  605.           </surface>
  606.         </collision>
  607.         <visual name='Wall_20_Visual'>
  608.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  609.           <geometry>
  610.             <box>
  611.               <size>7.5 0.2 2.5</size>
  612.             </box>
  613.           </geometry>
  614.           <material>
  615.             <script>
  616.               <uri>file://media/materials/scripts/gazebo.material</uri>
  617.               <name>Gazebo/Bricks</name>
  618.             </script>
  619.             <ambient>1 1 1 1</ambient>
  620.           </material>
  621.           <meta>
  622.             <layer>0</layer>
  623.           </meta>
  624.         </visual>
  625.         <pose frame=''>3.75542 1.77383 0 0 -0 -1.5708</pose>
  626.         <self_collide>0</self_collide>
  627.         <enable_wind>0</enable_wind>
  628.         <kinematic>0</kinematic>
  629.       </link>
  630.       <link name='Wall_21'>
  631.         <collision name='Wall_21_Collision'>
  632.           <geometry>
  633.             <box>
  634.               <size>3.75 0.2 2.5</size>
  635.             </box>
  636.           </geometry>
  637.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  638.           <max_contacts>10</max_contacts>
  639.           <surface>
  640.             <contact>
  641.               <ode/>
  642.             </contact>
  643.             <bounce/>
  644.             <friction>
  645.               <torsional>
  646.                 <ode/>
  647.               </torsional>
  648.               <ode/>
  649.             </friction>
  650.           </surface>
  651.         </collision>
  652.         <visual name='Wall_21_Visual'>
  653.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  654.           <geometry>
  655.             <box>
  656.               <size>3.75 0.2 2.5</size>
  657.             </box>
  658.           </geometry>
  659.           <material>
  660.             <script>
  661.               <uri>file://media/materials/scripts/gazebo.material</uri>
  662.               <name>Gazebo/Bricks</name>
  663.             </script>
  664.             <ambient>1 1 1 1</ambient>
  665.           </material>
  666.           <meta>
  667.             <layer>0</layer>
  668.           </meta>
  669.         </visual>
  670.         <pose frame=''>5.60195 -1.85465 0 0 -0 0</pose>
  671.         <self_collide>0</self_collide>
  672.         <enable_wind>0</enable_wind>
  673.         <kinematic>0</kinematic>
  674.       </link>
  675.       <link name='Wall_23'>
  676.         <collision name='Wall_23_Collision'>
  677.           <geometry>
  678.             <box>
  679.               <size>7.5 0.2 2.5</size>
  680.             </box>
  681.           </geometry>
  682.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  683.           <max_contacts>10</max_contacts>
  684.           <surface>
  685.             <contact>
  686.               <ode/>
  687.             </contact>
  688.             <bounce/>
  689.             <friction>
  690.               <torsional>
  691.                 <ode/>
  692.               </torsional>
  693.               <ode/>
  694.             </friction>
  695.           </surface>
  696.         </collision>
  697.         <visual name='Wall_23_Visual'>
  698.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  699.           <geometry>
  700.             <box>
  701.               <size>7.5 0.2 2.5</size>
  702.             </box>
  703.           </geometry>
  704.           <material>
  705.             <script>
  706.               <uri>file://media/materials/scripts/gazebo.material</uri>
  707.               <name>Gazebo/Bricks</name>
  708.             </script>
  709.             <ambient>1 1 1 1</ambient>
  710.           </material>
  711.           <meta>
  712.             <layer>0</layer>
  713.           </meta>
  714.         </visual>
  715.         <pose frame=''>7.40195 -5.52965 0 0 -0 -1.5708</pose>
  716.         <self_collide>0</self_collide>
  717.         <enable_wind>0</enable_wind>
  718.         <kinematic>0</kinematic>
  719.       </link>
  720.       <link name='Wall_25'>
  721.         <collision name='Wall_25_Collision'>
  722.           <geometry>
  723.             <box>
  724.               <size>3.75 0.2 2.5</size>
  725.             </box>
  726.           </geometry>
  727.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  728.           <max_contacts>10</max_contacts>
  729.           <surface>
  730.             <contact>
  731.               <ode/>
  732.             </contact>
  733.             <bounce/>
  734.             <friction>
  735.               <torsional>
  736.                 <ode/>
  737.               </torsional>
  738.               <ode/>
  739.             </friction>
  740.           </surface>
  741.         </collision>
  742.         <visual name='Wall_25_Visual'>
  743.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  744.           <geometry>
  745.             <box>
  746.               <size>3.75 0.2 2.5</size>
  747.             </box>
  748.           </geometry>
  749.           <material>
  750.             <script>
  751.               <uri>file://media/materials/scripts/gazebo.material</uri>
  752.               <name>Gazebo/Bricks</name>
  753.             </script>
  754.             <ambient>1 1 1 1</ambient>
  755.           </material>
  756.           <meta>
  757.             <layer>0</layer>
  758.           </meta>
  759.         </visual>
  760.         <pose frame=''>-3.76379 -7.45273 0 0 -0 1.5708</pose>
  761.         <self_collide>0</self_collide>
  762.         <enable_wind>0</enable_wind>
  763.         <kinematic>0</kinematic>
  764.       </link>
  765.       <link name='Wall_26'>
  766.         <collision name='Wall_26_Collision'>
  767.           <geometry>
  768.             <box>
  769.               <size>7.75 0.2 2.5</size>
  770.             </box>
  771.           </geometry>
  772.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  773.           <max_contacts>10</max_contacts>
  774.           <surface>
  775.             <contact>
  776.               <ode/>
  777.             </contact>
  778.             <bounce/>
  779.             <friction>
  780.               <torsional>
  781.                 <ode/>
  782.               </torsional>
  783.               <ode/>
  784.             </friction>
  785.           </surface>
  786.         </collision>
  787.         <visual name='Wall_26_Visual'>
  788.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  789.           <geometry>
  790.             <box>
  791.               <size>7.75 0.2 2.5</size>
  792.             </box>
  793.           </geometry>
  794.           <material>
  795.             <script>
  796.               <uri>file://media/materials/scripts/gazebo.material</uri>
  797.               <name>Gazebo/Bricks</name>
  798.             </script>
  799.             <ambient>1 1 1 1</ambient>
  800.           </material>
  801.           <meta>
  802.             <layer>0</layer>
  803.           </meta>
  804.         </visual>
  805.         <pose frame=''>0.036208 -5.65273 0 0 -0 0</pose>
  806.         <self_collide>0</self_collide>
  807.         <enable_wind>0</enable_wind>
  808.         <kinematic>0</kinematic>
  809.       </link>
  810.       <link name='Wall_28'>
  811.         <collision name='Wall_28_Collision'>
  812.           <geometry>
  813.             <box>
  814.               <size>4 0.2 2.5</size>
  815.             </box>
  816.           </geometry>
  817.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  818.           <max_contacts>10</max_contacts>
  819.           <surface>
  820.             <contact>
  821.               <ode/>
  822.             </contact>
  823.             <bounce/>
  824.             <friction>
  825.               <torsional>
  826.                 <ode/>
  827.               </torsional>
  828.               <ode/>
  829.             </friction>
  830.           </surface>
  831.         </collision>
  832.         <visual name='Wall_28_Visual'>
  833.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  834.           <geometry>
  835.             <box>
  836.               <size>4 0.2 2.5</size>
  837.             </box>
  838.           </geometry>
  839.           <material>
  840.             <script>
  841.               <uri>file://media/materials/scripts/gazebo.material</uri>
  842.               <name>Gazebo/Bricks</name>
  843.             </script>
  844.             <ambient>1 1 1 1</ambient>
  845.           </material>
  846.           <meta>
  847.             <layer>0</layer>
  848.           </meta>
  849.         </visual>
  850.         <pose frame=''>11.1704 -7.37425 0 0 -0 1.5708</pose>
  851.         <self_collide>0</self_collide>
  852.         <enable_wind>0</enable_wind>
  853.         <kinematic>0</kinematic>
  854.       </link>
  855.       <link name='Wall_30'>
  856.         <collision name='Wall_30_Collision'>
  857.           <geometry>
  858.             <box>
  859.               <size>4 0.2 2.5</size>
  860.             </box>
  861.           </geometry>
  862.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  863.           <max_contacts>10</max_contacts>
  864.           <surface>
  865.             <contact>
  866.               <ode/>
  867.             </contact>
  868.             <bounce/>
  869.             <friction>
  870.               <torsional>
  871.                 <ode/>
  872.               </torsional>
  873.               <ode/>
  874.             </friction>
  875.           </surface>
  876.         </collision>
  877.         <visual name='Wall_30_Visual'>
  878.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  879.           <geometry>
  880.             <box>
  881.               <size>4 0.2 2.5</size>
  882.             </box>
  883.           </geometry>
  884.           <material>
  885.             <script>
  886.               <uri>file://media/materials/scripts/gazebo.material</uri>
  887.               <name>Gazebo/Bricks</name>
  888.             </script>
  889.             <ambient>1 1 1 1</ambient>
  890.           </material>
  891.           <meta>
  892.             <layer>0</layer>
  893.           </meta>
  894.         </visual>
  895.         <pose frame=''>9.28042 1.81996 0 0 -0 0</pose>
  896.         <self_collide>0</self_collide>
  897.         <enable_wind>0</enable_wind>
  898.         <kinematic>0</kinematic>
  899.       </link>
  900.       <link name='Wall_32'>
  901.         <collision name='Wall_32_Collision'>
  902.           <geometry>
  903.             <box>
  904.               <size>4 0.2 2.5</size>
  905.             </box>
  906.           </geometry>
  907.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  908.           <max_contacts>10</max_contacts>
  909.           <surface>
  910.             <contact>
  911.               <ode/>
  912.             </contact>
  913.             <bounce/>
  914.             <friction>
  915.               <torsional>
  916.                 <ode/>
  917.               </torsional>
  918.               <ode/>
  919.             </friction>
  920.           </surface>
  921.         </collision>
  922.         <visual name='Wall_32_Visual'>
  923.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  924.           <geometry>
  925.             <box>
  926.               <size>4 0.2 2.5</size>
  927.             </box>
  928.           </geometry>
  929.           <material>
  930.             <script>
  931.               <uri>file://media/materials/scripts/gazebo.material</uri>
  932.               <name>Gazebo/Bricks</name>
  933.             </script>
  934.             <ambient>1 1 1 1</ambient>
  935.           </material>
  936.           <meta>
  937.             <layer>0</layer>
  938.           </meta>
  939.         </visual>
  940.         <pose frame=''>13.0603 5.5884 0 0 -0 3.14159</pose>
  941.         <self_collide>0</self_collide>
  942.         <enable_wind>0</enable_wind>
  943.         <kinematic>0</kinematic>
  944.       </link>
  945.       <link name='Wall_33'>
  946.         <collision name='Wall_33_Collision'>
  947.           <geometry>
  948.             <box>
  949.               <size>7.70353 0.2 2.5</size>
  950.             </box>
  951.           </geometry>
  952.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  953.           <max_contacts>10</max_contacts>
  954.           <surface>
  955.             <contact>
  956.               <ode/>
  957.             </contact>
  958.             <bounce/>
  959.             <friction>
  960.               <torsional>
  961.                 <ode/>
  962.               </torsional>
  963.               <ode/>
  964.             </friction>
  965.           </surface>
  966.         </collision>
  967.         <visual name='Wall_33_Visual'>
  968.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  969.           <geometry>
  970.             <box>
  971.               <size>7.70353 0.2 2.5</size>
  972.             </box>
  973.           </geometry>
  974.           <material>
  975.             <script>
  976.               <uri>file://media/materials/scripts/gazebo.material</uri>
  977.               <name>Gazebo/Bricks</name>
  978.             </script>
  979.             <ambient>1 1 1 1</ambient>
  980.           </material>
  981.           <meta>
  982.             <layer>0</layer>
  983.           </meta>
  984.         </visual>
  985.         <pose frame=''>11.1499 1.81166 0 0 -0 -1.56694</pose>
  986.         <self_collide>0</self_collide>
  987.         <enable_wind>0</enable_wind>
  988.         <kinematic>0</kinematic>
  989.       </link>
  990.       <link name='Wall_34'>
  991.         <collision name='Wall_34_Collision'>
  992.           <geometry>
  993.             <box>
  994.               <size>4 0.2 2.5</size>
  995.             </box>
  996.           </geometry>
  997.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  998.           <max_contacts>10</max_contacts>
  999.           <surface>
  1000.             <contact>
  1001.               <ode/>
  1002.             </contact>
  1003.             <bounce/>
  1004.             <friction>
  1005.               <torsional>
  1006.                 <ode/>
  1007.               </torsional>
  1008.               <ode/>
  1009.             </friction>
  1010.           </surface>
  1011.         </collision>
  1012.         <visual name='Wall_34_Visual'>
  1013.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1014.           <geometry>
  1015.             <box>
  1016.               <size>4 0.2 2.5</size>
  1017.             </box>
  1018.           </geometry>
  1019.           <material>
  1020.             <script>
  1021.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1022.               <name>Gazebo/Bricks</name>
  1023.             </script>
  1024.             <ambient>1 1 1 1</ambient>
  1025.           </material>
  1026.           <meta>
  1027.             <layer>0</layer>
  1028.           </meta>
  1029.         </visual>
  1030.         <pose frame=''>13.0895 -1.96508 0 0 -0 0</pose>
  1031.         <self_collide>0</self_collide>
  1032.         <enable_wind>0</enable_wind>
  1033.         <kinematic>0</kinematic>
  1034.       </link>
  1035.       <link name='Wall_35'>
  1036.         <collision name='Wall_35_Collision'>
  1037.           <geometry>
  1038.             <box>
  1039.               <size>4 0.2 2.5</size>
  1040.             </box>
  1041.           </geometry>
  1042.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1043.           <max_contacts>10</max_contacts>
  1044.           <surface>
  1045.             <contact>
  1046.               <ode/>
  1047.             </contact>
  1048.             <bounce/>
  1049.             <friction>
  1050.               <torsional>
  1051.                 <ode/>
  1052.               </torsional>
  1053.               <ode/>
  1054.             </friction>
  1055.           </surface>
  1056.         </collision>
  1057.         <visual name='Wall_35_Visual'>
  1058.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1059.           <geometry>
  1060.             <box>
  1061.               <size>4 0.2 2.5</size>
  1062.             </box>
  1063.           </geometry>
  1064.           <material>
  1065.             <script>
  1066.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1067.               <name>Gazebo/Bricks</name>
  1068.             </script>
  1069.             <ambient>1 1 1 1</ambient>
  1070.           </material>
  1071.           <meta>
  1072.             <layer>0</layer>
  1073.           </meta>
  1074.         </visual>
  1075.         <pose frame=''>14.968 -0.040077 0 0 -0 1.5708</pose>
  1076.         <self_collide>0</self_collide>
  1077.         <enable_wind>0</enable_wind>
  1078.         <kinematic>0</kinematic>
  1079.       </link>
  1080.       <link name='Wall_36'>
  1081.         <collision name='Wall_36_Collision'>
  1082.           <geometry>
  1083.             <box>
  1084.               <size>3.72116 0.2 2.5</size>
  1085.             </box>
  1086.           </geometry>
  1087.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1088.           <max_contacts>10</max_contacts>
  1089.           <surface>
  1090.             <contact>
  1091.               <ode/>
  1092.             </contact>
  1093.             <bounce/>
  1094.             <friction>
  1095.               <torsional>
  1096.                 <ode/>
  1097.               </torsional>
  1098.               <ode/>
  1099.             </friction>
  1100.           </surface>
  1101.         </collision>
  1102.         <visual name='Wall_36_Visual'>
  1103.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1104.           <geometry>
  1105.             <box>
  1106.               <size>3.72116 0.2 2.5</size>
  1107.             </box>
  1108.           </geometry>
  1109.           <material>
  1110.             <script>
  1111.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1112.               <name>Gazebo/Bricks</name>
  1113.             </script>
  1114.             <ambient>1 1 1 1</ambient>
  1115.           </material>
  1116.           <meta>
  1117.             <layer>0</layer>
  1118.           </meta>
  1119.         </visual>
  1120.         <pose frame=''>16.7999 1.86166 0 0 -0 -0.013028</pose>
  1121.         <self_collide>0</self_collide>
  1122.         <enable_wind>0</enable_wind>
  1123.         <kinematic>0</kinematic>
  1124.       </link>
  1125.       <link name='Wall_38'>
  1126.         <collision name='Wall_38_Collision'>
  1127.           <geometry>
  1128.             <box>
  1129.               <size>4 0.2 2.5</size>
  1130.             </box>
  1131.           </geometry>
  1132.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1133.           <max_contacts>10</max_contacts>
  1134.           <surface>
  1135.             <contact>
  1136.               <ode/>
  1137.             </contact>
  1138.             <bounce/>
  1139.             <friction>
  1140.               <torsional>
  1141.                 <ode/>
  1142.               </torsional>
  1143.               <ode/>
  1144.             </friction>
  1145.           </surface>
  1146.         </collision>
  1147.         <visual name='Wall_38_Visual'>
  1148.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1149.           <geometry>
  1150.             <box>
  1151.               <size>4 0.2 2.5</size>
  1152.             </box>
  1153.           </geometry>
  1154.           <material>
  1155.             <script>
  1156.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1157.               <name>Gazebo/Bricks</name>
  1158.             </script>
  1159.             <ambient>1 1 1 1</ambient>
  1160.           </material>
  1161.           <meta>
  1162.             <layer>0</layer>
  1163.           </meta>
  1164.         </visual>
  1165.         <pose frame=''>-11.1611 7.38531 0 0 -0 -1.5708</pose>
  1166.         <self_collide>0</self_collide>
  1167.         <enable_wind>0</enable_wind>
  1168.         <kinematic>0</kinematic>
  1169.       </link>
  1170.       <link name='Wall_39'>
  1171.         <collision name='Wall_39_Collision'>
  1172.           <geometry>
  1173.             <box>
  1174.               <size>3.84305 0.2 2.5</size>
  1175.             </box>
  1176.           </geometry>
  1177.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1178.           <max_contacts>10</max_contacts>
  1179.           <surface>
  1180.             <contact>
  1181.               <ode/>
  1182.             </contact>
  1183.             <bounce/>
  1184.             <friction>
  1185.               <torsional>
  1186.                 <ode/>
  1187.               </torsional>
  1188.               <ode/>
  1189.             </friction>
  1190.           </surface>
  1191.         </collision>
  1192.         <visual name='Wall_39_Visual'>
  1193.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1194.           <geometry>
  1195.             <box>
  1196.               <size>3.84305 0.2 2.5</size>
  1197.             </box>
  1198.           </geometry>
  1199.           <material>
  1200.             <script>
  1201.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1202.               <name>Gazebo/Bricks</name>
  1203.             </script>
  1204.             <ambient>1 1 1 1</ambient>
  1205.           </material>
  1206.           <meta>
  1207.             <layer>0</layer>
  1208.           </meta>
  1209.         </visual>
  1210.         <pose frame=''>-9.31457 5.46031 0 0 -0 0</pose>
  1211.         <self_collide>0</self_collide>
  1212.         <enable_wind>0</enable_wind>
  1213.         <kinematic>0</kinematic>
  1214.       </link>
  1215.       <link name='Wall_4'>
  1216.         <collision name='Wall_4_Collision'>
  1217.           <geometry>
  1218.             <box>
  1219.               <size>37.3438 0.2 2.5</size>
  1220.             </box>
  1221.           </geometry>
  1222.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1223.           <max_contacts>10</max_contacts>
  1224.           <surface>
  1225.             <contact>
  1226.               <ode/>
  1227.             </contact>
  1228.             <bounce/>
  1229.             <friction>
  1230.               <torsional>
  1231.                 <ode/>
  1232.               </torsional>
  1233.               <ode/>
  1234.             </friction>
  1235.           </surface>
  1236.         </collision>
  1237.         <visual name='Wall_4_Visual'>
  1238.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1239.           <geometry>
  1240.             <box>
  1241.               <size>37.3438 0.2 2.5</size>
  1242.             </box>
  1243.           </geometry>
  1244.           <material>
  1245.             <script>
  1246.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1247.               <name>Gazebo/Bricks</name>
  1248.             </script>
  1249.             <ambient>1 1 1 1</ambient>
  1250.           </material>
  1251.           <meta>
  1252.             <layer>0</layer>
  1253.           </meta>
  1254.         </visual>
  1255.         <pose frame=''>-0.023062 -9.27664 0 0 -0 -3.14113</pose>
  1256.         <self_collide>0</self_collide>
  1257.         <enable_wind>0</enable_wind>
  1258.         <kinematic>0</kinematic>
  1259.       </link>
  1260.       <link name='Wall_40'>
  1261.         <collision name='Wall_40_Collision'>
  1262.           <geometry>
  1263.             <box>
  1264.               <size>3.75 0.2 2.5</size>
  1265.             </box>
  1266.           </geometry>
  1267.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1268.           <max_contacts>10</max_contacts>
  1269.           <surface>
  1270.             <contact>
  1271.               <ode/>
  1272.             </contact>
  1273.             <bounce/>
  1274.             <friction>
  1275.               <torsional>
  1276.                 <ode/>
  1277.               </torsional>
  1278.               <ode/>
  1279.             </friction>
  1280.           </surface>
  1281.         </collision>
  1282.         <visual name='Wall_40_Visual'>
  1283.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1284.           <geometry>
  1285.             <box>
  1286.               <size>3.75 0.2 2.5</size>
  1287.             </box>
  1288.           </geometry>
  1289.           <material>
  1290.             <script>
  1291.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1292.               <name>Gazebo/Bricks</name>
  1293.             </script>
  1294.             <ambient>1 1 1 1</ambient>
  1295.           </material>
  1296.           <meta>
  1297.             <layer>0</layer>
  1298.           </meta>
  1299.         </visual>
  1300.         <pose frame=''>-7.51457 3.66031 0 0 -0 -1.5708</pose>
  1301.         <self_collide>0</self_collide>
  1302.         <enable_wind>0</enable_wind>
  1303.         <kinematic>0</kinematic>
  1304.       </link>
  1305.       <link name='Wall_42'>
  1306.         <collision name='Wall_42_Collision'>
  1307.           <geometry>
  1308.             <box>
  1309.               <size>7.75 0.2 2.5</size>
  1310.             </box>
  1311.           </geometry>
  1312.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1313.           <max_contacts>10</max_contacts>
  1314.           <surface>
  1315.             <contact>
  1316.               <ode/>
  1317.             </contact>
  1318.             <bounce/>
  1319.             <friction>
  1320.               <torsional>
  1321.                 <ode/>
  1322.               </torsional>
  1323.               <ode/>
  1324.             </friction>
  1325.           </surface>
  1326.         </collision>
  1327.         <visual name='Wall_42_Visual'>
  1328.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1329.           <geometry>
  1330.             <box>
  1331.               <size>7.75 0.2 2.5</size>
  1332.             </box>
  1333.           </geometry>
  1334.           <material>
  1335.             <script>
  1336.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1337.               <name>Gazebo/Bricks</name>
  1338.             </script>
  1339.             <ambient>1 1 1 1</ambient>
  1340.           </material>
  1341.           <meta>
  1342.             <layer>0</layer>
  1343.           </meta>
  1344.         </visual>
  1345.         <pose frame=''>-3.76379 5.46379 0 0 -0 -1.5708</pose>
  1346.         <self_collide>0</self_collide>
  1347.         <enable_wind>0</enable_wind>
  1348.         <kinematic>0</kinematic>
  1349.       </link>
  1350.       <link name='Wall_44'>
  1351.         <collision name='Wall_44_Collision'>
  1352.           <geometry>
  1353.             <box>
  1354.               <size>4 0.2 2.5</size>
  1355.             </box>
  1356.           </geometry>
  1357.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1358.           <max_contacts>10</max_contacts>
  1359.           <surface>
  1360.             <contact>
  1361.               <ode/>
  1362.             </contact>
  1363.             <bounce/>
  1364.             <friction>
  1365.               <torsional>
  1366.                 <ode/>
  1367.               </torsional>
  1368.               <ode/>
  1369.             </friction>
  1370.           </surface>
  1371.         </collision>
  1372.         <visual name='Wall_44_Visual'>
  1373.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1374.           <geometry>
  1375.             <box>
  1376.               <size>4 0.2 2.5</size>
  1377.             </box>
  1378.           </geometry>
  1379.           <material>
  1380.             <script>
  1381.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1382.               <name>Gazebo/Bricks</name>
  1383.             </script>
  1384.             <ambient>1 1 1 1</ambient>
  1385.           </material>
  1386.           <meta>
  1387.             <layer>0</layer>
  1388.           </meta>
  1389.         </visual>
  1390.         <pose frame=''>-1.83879 5.54188 0 0 -0 0</pose>
  1391.         <self_collide>0</self_collide>
  1392.         <enable_wind>0</enable_wind>
  1393.         <kinematic>0</kinematic>
  1394.       </link>
  1395.       <link name='Wall_5'>
  1396.         <collision name='Wall_5_Collision'>
  1397.           <geometry>
  1398.             <box>
  1399.               <size>7.5 0.2 2.5</size>
  1400.             </box>
  1401.           </geometry>
  1402.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1403.           <max_contacts>10</max_contacts>
  1404.           <surface>
  1405.             <contact>
  1406.               <ode/>
  1407.             </contact>
  1408.             <bounce/>
  1409.             <friction>
  1410.               <torsional>
  1411.                 <ode/>
  1412.               </torsional>
  1413.               <ode/>
  1414.             </friction>
  1415.           </surface>
  1416.         </collision>
  1417.         <visual name='Wall_5_Visual'>
  1418.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1419.           <geometry>
  1420.             <box>
  1421.               <size>7.5 0.2 2.5</size>
  1422.             </box>
  1423.           </geometry>
  1424.           <material>
  1425.             <script>
  1426.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1427.               <name>Gazebo/Bricks</name>
  1428.             </script>
  1429.             <ambient>1 1 1 1</ambient>
  1430.           </material>
  1431.           <meta>
  1432.             <layer>0</layer>
  1433.           </meta>
  1434.         </visual>
  1435.         <pose frame=''>-18.6196 -5.61033 0 0 -0 1.5708</pose>
  1436.         <self_collide>0</self_collide>
  1437.         <enable_wind>0</enable_wind>
  1438.         <kinematic>0</kinematic>
  1439.       </link>
  1440.       <link name='Wall_7'>
  1441.         <collision name='Wall_7_Collision'>
  1442.           <geometry>
  1443.             <box>
  1444.               <size>11.5 0.2 2.5</size>
  1445.             </box>
  1446.           </geometry>
  1447.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1448.           <max_contacts>10</max_contacts>
  1449.           <surface>
  1450.             <contact>
  1451.               <ode/>
  1452.             </contact>
  1453.             <bounce/>
  1454.             <friction>
  1455.               <torsional>
  1456.                 <ode/>
  1457.               </torsional>
  1458.               <ode/>
  1459.             </friction>
  1460.           </surface>
  1461.         </collision>
  1462.         <visual name='Wall_7_Visual'>
  1463.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1464.           <geometry>
  1465.             <box>
  1466.               <size>11.5 0.2 2.5</size>
  1467.             </box>
  1468.           </geometry>
  1469.           <material>
  1470.             <script>
  1471.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1472.               <name>Gazebo/Bricks</name>
  1473.             </script>
  1474.             <ambient>1 1 1 1</ambient>
  1475.           </material>
  1476.           <meta>
  1477.             <layer>0</layer>
  1478.           </meta>
  1479.         </visual>
  1480.         <pose frame=''>-14.9546 -0.004511 0 0 -0 -1.5708</pose>
  1481.         <self_collide>0</self_collide>
  1482.         <enable_wind>0</enable_wind>
  1483.         <kinematic>0</kinematic>
  1484.       </link>
  1485.       <link name='Wall_8'>
  1486.         <collision name='Wall_8_Collision'>
  1487.           <geometry>
  1488.             <box>
  1489.               <size>3.9604 0.2 2.5</size>
  1490.             </box>
  1491.           </geometry>
  1492.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1493.           <max_contacts>10</max_contacts>
  1494.           <surface>
  1495.             <contact>
  1496.               <ode/>
  1497.             </contact>
  1498.             <bounce/>
  1499.             <friction>
  1500.               <torsional>
  1501.                 <ode/>
  1502.               </torsional>
  1503.               <ode/>
  1504.             </friction>
  1505.           </surface>
  1506.         </collision>
  1507.         <visual name='Wall_8_Visual'>
  1508.           <pose frame=''>0 0 1.25 0 -0 0</pose>
  1509.           <geometry>
  1510.             <box>
  1511.               <size>3.9604 0.2 2.5</size>
  1512.             </box>
  1513.           </geometry>
  1514.           <material>
  1515.             <script>
  1516.               <uri>file://media/materials/scripts/gazebo.material</uri>
  1517.               <name>Gazebo/Bricks</name>
  1518.             </script>
  1519.             <ambient>1 1 1 1</ambient>
  1520.           </material>
  1521.           <meta>
  1522.             <layer>0</layer>
  1523.           </meta>
  1524.         </visual>
  1525.         <pose frame=''>-13.0498 -5.63906 0 0 -0 0.021236</pose>
  1526.         <self_collide>0</self_collide>
  1527.         <enable_wind>0</enable_wind>
  1528.         <kinematic>0</kinematic>
  1529.       </link>
  1530.       <static>1</static>
  1531.     </model>
  1532.     <state world_name='default'>
  1533.       <sim_time>493 628000000</sim_time>
  1534.       <real_time>421 177324053</real_time>
  1535.       <wall_time>1659794100 482234418</wall_time>
  1536.       <iterations>420094</iterations>
  1537.       <model name='map2_1'>
  1538.         <pose frame=''>1.79516 0.071786 0 0 -0 0</pose>
  1539.         <scale>1 1 1</scale>
  1540.         <link name='Wall_0'>
  1541.           <pose frame=''>-16.8244 5.58646 0 0 -0 1.5708</pose>
  1542.           <velocity>0 0 0 0 -0 0</velocity>
  1543.           <acceleration>0 0 0 0 -0 0</acceleration>
  1544.           <wrench>0 0 0 0 -0 0</wrench>
  1545.         </link>
  1546.         <link name='Wall_1'>
  1547.           <pose frame=''>1.79536 9.38646 0 0 -0 0</pose>
  1548.           <velocity>0 0 0 0 -0 0</velocity>
  1549.           <acceleration>0 0 0 0 -0 0</acceleration>
  1550.           <wrench>0 0 0 0 -0 0</wrench>
  1551.         </link>
  1552.         <link name='Wall_10'>
  1553.           <pose frame=''>-11.2413 1.91501 0 0 0 -0.012287</pose>
  1554.           <velocity>0 0 0 0 -0 0</velocity>
  1555.           <acceleration>0 0 0 0 -0 0</acceleration>
  1556.           <wrench>0 0 0 0 -0 0</wrench>
  1557.         </link>
  1558.         <link name='Wall_11'>
  1559.           <pose frame=''>-9.37154 -0.001304 0 0 0 -1.58339</pose>
  1560.           <velocity>0 0 0 0 -0 0</velocity>
  1561.           <acceleration>0 0 0 0 -0 0</acceleration>
  1562.           <wrench>0 0 0 0 -0 0</wrench>
  1563.         </link>
  1564.         <link name='Wall_12'>
  1565.           <pose frame=''>-3.8448 -1.89434 0 0 -0 0</pose>
  1566.           <velocity>0 0 0 0 -0 0</velocity>
  1567.           <acceleration>0 0 0 0 -0 0</acceleration>
  1568.           <wrench>0 0 0 0 -0 0</wrench>
  1569.         </link>
  1570.         <link name='Wall_14'>
  1571.           <pose frame=''>1.7052 0.030656 0 0 -0 1.5708</pose>
  1572.           <velocity>0 0 0 0 -0 0</velocity>
  1573.           <acceleration>0 0 0 0 -0 0</acceleration>
  1574.           <wrench>0 0 0 0 -0 0</wrench>
  1575.         </link>
  1576.         <link name='Wall_15'>
  1577.           <pose frame=''>3.6302 1.95566 0 0 -0 0</pose>
  1578.           <velocity>0 0 0 0 -0 0</velocity>
  1579.           <acceleration>0 0 0 0 -0 0</acceleration>
  1580.           <wrench>0 0 0 0 -0 0</wrench>
  1581.         </link>
  1582.         <link name='Wall_17'>
  1583.           <pose frame=''>-5.69055 -3.67674 0 0 0 -1.5708</pose>
  1584.           <velocity>0 0 0 0 -0 0</velocity>
  1585.           <acceleration>0 0 0 0 -0 0</acceleration>
  1586.           <wrench>0 0 0 0 -0 0</wrench>
  1587.         </link>
  1588.         <link name='Wall_19'>
  1589.           <pose frame=''>7.44363 5.61367 0 0 0 -3.1164</pose>
  1590.           <velocity>0 0 0 0 -0 0</velocity>
  1591.           <acceleration>0 0 0 0 -0 0</acceleration>
  1592.           <wrench>0 0 0 0 -0 0</wrench>
  1593.         </link>
  1594.         <link name='Wall_2'>
  1595.           <pose frame=''>20.3454 0.048616 0 0 0 -1.5733</pose>
  1596.           <velocity>0 0 0 0 -0 0</velocity>
  1597.           <acceleration>0 0 0 0 -0 0</acceleration>
  1598.           <wrench>0 0 0 0 -0 0</wrench>
  1599.         </link>
  1600.         <link name='Wall_20'>
  1601.           <pose frame=''>5.55058 1.84562 0 0 0 -1.5708</pose>
  1602.           <velocity>0 0 0 0 -0 0</velocity>
  1603.           <acceleration>0 0 0 0 -0 0</acceleration>
  1604.           <wrench>0 0 0 0 -0 0</wrench>
  1605.         </link>
  1606.         <link name='Wall_21'>
  1607.           <pose frame=''>7.39711 -1.78286 0 0 -0 0</pose>
  1608.           <velocity>0 0 0 0 -0 0</velocity>
  1609.           <acceleration>0 0 0 0 -0 0</acceleration>
  1610.           <wrench>0 0 0 0 -0 0</wrench>
  1611.         </link>
  1612.         <link name='Wall_23'>
  1613.           <pose frame=''>9.19711 -5.45784 0 0 0 -1.5708</pose>
  1614.           <velocity>0 0 0 0 -0 0</velocity>
  1615.           <acceleration>0 0 0 0 -0 0</acceleration>
  1616.           <wrench>0 0 0 0 -0 0</wrench>
  1617.         </link>
  1618.         <link name='Wall_25'>
  1619.           <pose frame=''>-1.96863 -7.38094 0 0 -0 1.5708</pose>
  1620.           <velocity>0 0 0 0 -0 0</velocity>
  1621.           <acceleration>0 0 0 0 -0 0</acceleration>
  1622.           <wrench>0 0 0 0 -0 0</wrench>
  1623.         </link>
  1624.         <link name='Wall_26'>
  1625.           <pose frame=''>1.83137 -5.58094 0 0 -0 0</pose>
  1626.           <velocity>0 0 0 0 -0 0</velocity>
  1627.           <acceleration>0 0 0 0 -0 0</acceleration>
  1628.           <wrench>0 0 0 0 -0 0</wrench>
  1629.         </link>
  1630.         <link name='Wall_28'>
  1631.           <pose frame=''>12.9656 -7.30244 0 0 -0 1.5708</pose>
  1632.           <velocity>0 0 0 0 -0 0</velocity>
  1633.           <acceleration>0 0 0 0 -0 0</acceleration>
  1634.           <wrench>0 0 0 0 -0 0</wrench>
  1635.         </link>
  1636.         <link name='Wall_30'>
  1637.           <pose frame=''>11.0756 1.89175 0 0 -0 0</pose>
  1638.           <velocity>0 0 0 0 -0 0</velocity>
  1639.           <acceleration>0 0 0 0 -0 0</acceleration>
  1640.           <wrench>0 0 0 0 -0 0</wrench>
  1641.         </link>
  1642.         <link name='Wall_32'>
  1643.           <pose frame=''>14.8555 5.66019 0 0 -0 3.14159</pose>
  1644.           <velocity>0 0 0 0 -0 0</velocity>
  1645.           <acceleration>0 0 0 0 -0 0</acceleration>
  1646.           <wrench>0 0 0 0 -0 0</wrench>
  1647.         </link>
  1648.         <link name='Wall_33'>
  1649.           <pose frame=''>12.9451 1.88345 0 0 0 -1.56694</pose>
  1650.           <velocity>0 0 0 0 -0 0</velocity>
  1651.           <acceleration>0 0 0 0 -0 0</acceleration>
  1652.           <wrench>0 0 0 0 -0 0</wrench>
  1653.         </link>
  1654.         <link name='Wall_34'>
  1655.           <pose frame=''>14.8847 -1.89329 0 0 -0 0</pose>
  1656.           <velocity>0 0 0 0 -0 0</velocity>
  1657.           <acceleration>0 0 0 0 -0 0</acceleration>
  1658.           <wrench>0 0 0 0 -0 0</wrench>
  1659.         </link>
  1660.         <link name='Wall_35'>
  1661.           <pose frame=''>16.7632 0.031706 0 0 -0 1.5708</pose>
  1662.           <velocity>0 0 0 0 -0 0</velocity>
  1663.           <acceleration>0 0 0 0 -0 0</acceleration>
  1664.           <wrench>0 0 0 0 -0 0</wrench>
  1665.         </link>
  1666.         <link name='Wall_36'>
  1667.           <pose frame=''>18.5951 1.93345 0 0 0 -0.013028</pose>
  1668.           <velocity>0 0 0 0 -0 0</velocity>
  1669.           <acceleration>0 0 0 0 -0 0</acceleration>
  1670.           <wrench>0 0 0 0 -0 0</wrench>
  1671.         </link>
  1672.         <link name='Wall_38'>
  1673.           <pose frame=''>-9.36594 7.4571 0 0 0 -1.5708</pose>
  1674.           <velocity>0 0 0 0 -0 0</velocity>
  1675.           <acceleration>0 0 0 0 -0 0</acceleration>
  1676.           <wrench>0 0 0 0 -0 0</wrench>
  1677.         </link>
  1678.         <link name='Wall_39'>
  1679.           <pose frame=''>-7.51941 5.5321 0 0 -0 0</pose>
  1680.           <velocity>0 0 0 0 -0 0</velocity>
  1681.           <acceleration>0 0 0 0 -0 0</acceleration>
  1682.           <wrench>0 0 0 0 -0 0</wrench>
  1683.         </link>
  1684.         <link name='Wall_4'>
  1685.           <pose frame=''>1.7721 -9.20484 0 0 0 -3.14113</pose>
  1686.           <velocity>0 0 0 0 -0 0</velocity>
  1687.           <acceleration>0 0 0 0 -0 0</acceleration>
  1688.           <wrench>0 0 0 0 -0 0</wrench>
  1689.         </link>
  1690.         <link name='Wall_40'>
  1691.           <pose frame=''>-5.71941 3.7321 0 0 0 -1.5708</pose>
  1692.           <velocity>0 0 0 0 -0 0</velocity>
  1693.           <acceleration>0 0 0 0 -0 0</acceleration>
  1694.           <wrench>0 0 0 0 -0 0</wrench>
  1695.         </link>
  1696.         <link name='Wall_42'>
  1697.           <pose frame=''>-1.96863 5.53558 0 0 0 -1.5708</pose>
  1698.           <velocity>0 0 0 0 -0 0</velocity>
  1699.           <acceleration>0 0 0 0 -0 0</acceleration>
  1700.           <wrench>0 0 0 0 -0 0</wrench>
  1701.         </link>
  1702.         <link name='Wall_44'>
  1703.           <pose frame=''>-0.043632 5.61367 0 0 -0 0</pose>
  1704.           <velocity>0 0 0 0 -0 0</velocity>
  1705.           <acceleration>0 0 0 0 -0 0</acceleration>
  1706.           <wrench>0 0 0 0 -0 0</wrench>
  1707.         </link>
  1708.         <link name='Wall_5'>
  1709.           <pose frame=''>-16.8244 -5.53854 0 0 -0 1.5708</pose>
  1710.           <velocity>0 0 0 0 -0 0</velocity>
  1711.           <acceleration>0 0 0 0 -0 0</acceleration>
  1712.           <wrench>0 0 0 0 -0 0</wrench>
  1713.         </link>
  1714.         <link name='Wall_7'>
  1715.           <pose frame=''>-13.1594 0.067276 0 0 0 -1.5708</pose>
  1716.           <velocity>0 0 0 0 -0 0</velocity>
  1717.           <acceleration>0 0 0 0 -0 0</acceleration>
  1718.           <wrench>0 0 0 0 -0 0</wrench>
  1719.         </link>
  1720.         <link name='Wall_8'>
  1721.           <pose frame=''>-11.2546 -5.56724 0 0 -0 0.021236</pose>
  1722.           <velocity>0 0 0 0 -0 0</velocity>
  1723.           <acceleration>0 0 0 0 -0 0</acceleration>
  1724.           <wrench>0 0 0 0 -0 0</wrench>
  1725.         </link>
  1726.       </model>
  1727.       <model name='ground_plane'>
  1728.         <pose frame=''>0 0 0 0 -0 0</pose>
  1729.         <scale>1 1 1</scale>
  1730.         <link name='link'>
  1731.           <pose frame=''>0 0 0 0 -0 0</pose>
  1732.           <velocity>0 0 0 0 -0 0</velocity>
  1733.           <acceleration>0 0 0 0 -0 0</acceleration>
  1734.           <wrench>0 0 0 0 -0 0</wrench>
  1735.         </link>
  1736.       </model>
  1737.       <model name='landing2'>
  1738.         <pose frame=''>18.3012 -0.3 0 0 1.57 1.57</pose>
  1739.         <scale>1 1 1</scale>
  1740.         <link name='link'>
  1741.           <pose frame=''>18.3012 -0.3 0 0 1.57 1.57</pose>
  1742.           <velocity>0 0 0 0 -0 0</velocity>
  1743.           <acceleration>0 0 0 0 -0 0</acceleration>
  1744.           <wrench>0 0 0 0 -0 0</wrench>
  1745.         </link>
  1746.       </model>
  1747.       <model name='takeoff'>
  1748.         <pose frame=''>-16 -0.5 0 0 1.57 1.57</pose>
  1749.         <scale>1 1 1</scale>
  1750.         <link name='link'>
  1751.           <pose frame=''>-16 -0.5 0 0 1.57 1.57</pose>
  1752.           <velocity>0 0 0 0 -0 0</velocity>
  1753.           <acceleration>0 0 0 0 -0 0</acceleration>
  1754.           <wrench>0 0 0 0 -0 0</wrench>
  1755.         </link>
  1756.       </model>
  1757.       <model name='target_red'>
  1758.         <pose frame=''>-8.97289 -3.7558 0 0 -0 0</pose>
  1759.         <scale>1 1 1</scale>
  1760.         <link name='link'>
  1761.           <pose frame=''>-8.97289 -3.7558 0 0 -0 0</pose>
  1762.           <velocity>0 0 0 0 -0 0</velocity>
  1763.           <acceleration>0 0 0 0 -0 0</acceleration>
  1764.           <wrench>0 0 0 0 -0 0</wrench>
  1765.         </link>
  1766.       </model>
  1767.       <model name='target_blue'>
  1768.         <pose frame=''>10.9445 8.81946 0.18348 0 -0 0</pose>
  1769.         <scale>1 1 1</scale>
  1770.         <link name='link'>
  1771.           <pose frame=''>10.9445 8.81946 0.18348 0 -0 0</pose>
  1772.           <velocity>0 0 0 0 -0 0</velocity>
  1773.           <acceleration>0 0 0 0 -0 0</acceleration>
  1774.           <wrench>0 0 0 0 -0 0</wrench>
  1775.         </link>
  1776.       </model>
  1777.       <model name='target_green'>
  1778.         <pose frame=''>18.5856 -8.05752 0 0 -0 0</pose>
  1779.         <scale>1 1 1</scale>
  1780.         <link name='link'>
  1781.           <pose frame=''>18.5856 -8.05752 0 0 -0 0</pose>
  1782.           <velocity>0 0 0 0 -0 0</velocity>
  1783.           <acceleration>0 0 0 0 -0 0</acceleration>
  1784.           <wrench>0 0 0 0 -0 0</wrench>
  1785.         </link>
  1786.       </model>
  1787.       <light name='sun'>
  1788.         <pose frame=''>0 0 10 0 -0 0</pose>
  1789.       </light>
  1790.     </state>
  1791.     <gui fullscreen='0'>
  1792.       <camera name='user_camera'>
  1793.         <pose frame=''>16.8887 -5.88816 80.8158 0 1.398 2.67275</pose>
  1794.         <view_controller>orbit</view_controller>
  1795.         <projection_type>perspective</projection_type>
  1796.       </camera>
  1797.     </gui>
  1798.     <model name='landing2'>
  1799.       <static>1</static>
  1800.       <link name='link'>
  1801.         <visual name='visual'>
  1802.           <geometry>
  1803.             <mesh>
  1804.               <uri>model://landing2/meshes/landing2.dae</uri>
  1805.               <scale>2 4 4</scale>
  1806.             </mesh>
  1807.           </geometry>
  1808.         </visual>
  1809.         <collision name='Landing_2_Collision'>
  1810.           <geometry>
  1811.             <mesh>
  1812.               <uri>model://landing2/meshes/landing2.dae</uri>
  1813.               <scale>2 4 4</scale>
  1814.             </mesh>
  1815.           </geometry>
  1816.           <max_contacts>10</max_contacts>
  1817.           <surface>
  1818.             <contact>
  1819.               <ode/>
  1820.             </contact>
  1821.             <bounce/>
  1822.             <friction>
  1823.               <torsional>
  1824.                 <ode/>
  1825.               </torsional>
  1826.               <ode/>
  1827.             </friction>
  1828.           </surface>
  1829.         </collision>
  1830.         <collision name='Landing_2_Collision'>
  1831.           <geometry>
  1832.             <mesh>
  1833.               <uri>model://landing2/meshes/landing2.dae</uri>
  1834.               <scale>2 4 4</scale>
  1835.             </mesh>
  1836.           </geometry>
  1837.           <max_contacts>10</max_contacts>
  1838.           <surface>
  1839.             <contact>
  1840.               <ode/>
  1841.             </contact>
  1842.             <bounce/>
  1843.             <friction>
  1844.               <torsional>
  1845.                 <ode/>
  1846.               </torsional>
  1847.               <ode/>
  1848.             </friction>
  1849.           </surface>
  1850.         </collision>
  1851.         <self_collide>0</self_collide>
  1852.         <enable_wind>0</enable_wind>
  1853.         <kinematic>0</kinematic>
  1854.       </link>
  1855.       <pose frame=''>17 0 0 0 1.57 1.57</pose>
  1856.     </model>
  1857.     <model name='takeoff'>
  1858.       <static>1</static>
  1859.       <link name='link'>
  1860.         <visual name='visual'>
  1861.           <geometry>
  1862.             <mesh>
  1863.               <uri>model://takeoff/meshes/takeoff.dae</uri>
  1864.               <scale>2 2 2</scale>
  1865.             </mesh>
  1866.           </geometry>
  1867.         </visual>
  1868.         <collision name='Takeoff_Collision'>
  1869.           <geometry>
  1870.             <mesh>
  1871.               <uri>model://takeoff/meshes/takeoff.dae</uri>
  1872.               <scale>2 2 2</scale>
  1873.             </mesh>
  1874.           </geometry>
  1875.           <max_contacts>10</max_contacts>
  1876.           <surface>
  1877.             <contact>
  1878.               <ode/>
  1879.             </contact>
  1880.             <bounce/>
  1881.             <friction>
  1882.               <torsional>
  1883.                 <ode/>
  1884.               </torsional>
  1885.               <ode/>
  1886.             </friction>
  1887.           </surface>
  1888.         </collision>
  1889.         <self_collide>0</self_collide>
  1890.         <enable_wind>0</enable_wind>
  1891.         <kinematic>0</kinematic>
  1892.       </link>
  1893.       <pose frame=''>-18 -0.5 0 0 1.57 1.57</pose>
  1894.     </model>
  1895.  
  1896.     <model name='target_red'>
  1897.       <static>0</static>
  1898.       <link name='link'>
  1899.         <pose frame=''>0 0 0 0 0 0</pose>
  1900.         <collision name='Target_Red_Collision'>
  1901.           <geometry>
  1902.             <mesh>
  1903.               <uri>model://target_red/meshes/target_red.dae</uri>
  1904.               <scale>0.9 0.9 0.9</scale>
  1905.             </mesh>
  1906.           </geometry>
  1907.           <max_contacts>10</max_contacts>
  1908.           <surface>
  1909.             <contact>
  1910.               <ode/>
  1911.             </contact>
  1912.             <bounce/>
  1913.             <friction>
  1914.               <torsional>
  1915.                 <ode/>
  1916.               </torsional>
  1917.               <ode/>
  1918.             </friction>
  1919.           </surface>
  1920.           <pose frame=''>0 0 -0.225 0 0 3.1415</pose>
  1921.         </collision>
  1922.         <visual name='visual'>
  1923.           <geometry>
  1924.             <mesh>
  1925.               <uri>model://target_red/meshes/target_red.dae</uri>
  1926.               <scale>0.9 0.9 0.9</scale>
  1927.             </mesh>
  1928.           </geometry>
  1929.           <pose frame=''>0 0 -0.225 0 0 3.1415</pose>
  1930.         </visual>
  1931.         <self_collide>0</self_collide>
  1932.         <enable_wind>0</enable_wind>
  1933.         <kinematic>0</kinematic>
  1934.         <gravity>0</gravity>
  1935.       </link>
  1936.       <pose frame=''>-8.97289 -3.7558 0 0 -0 0</pose>
  1937.     </model>
  1938.  
  1939.     <model name='target_blue'>
  1940.       <static>0</static>
  1941.       <link name='link'>
  1942.        <pose frame=''>0 0 0 0 0 0</pose>
  1943.           <collision name='Target_Blue_Collision'>
  1944.             <geometry>
  1945.               <mesh>
  1946.                 <uri>model://target_blue/meshes/target_blue.dae</uri>
  1947.                 <scale>0.9 0.9 0.9</scale>
  1948.               </mesh>
  1949.             </geometry>
  1950.             <max_contacts>10</max_contacts>
  1951.             <surface>
  1952.               <contact>
  1953.                 <ode/>
  1954.               </contact>
  1955.               <bounce/>
  1956.               <friction>
  1957.                 <torsional>
  1958.                   <ode/>
  1959.                 </torsional>
  1960.                 <ode/>
  1961.               </friction>
  1962.             </surface>
  1963.           <pose frame=''>0 0 -0.225 0 0 3.1415</pose>
  1964.           </collision>
  1965.         <visual name='visual'>
  1966.           <geometry>
  1967.             <mesh>
  1968.               <uri>model://target_blue/meshes/target_blue.dae</uri>
  1969.               <scale>0.9 0.9 0.9</scale>
  1970.             </mesh>
  1971.           </geometry>
  1972.           <pose frame=''>0 0 -0.225 0 0 3.1415</pose>
  1973.         </visual>
  1974.         <self_collide>0</self_collide>
  1975.         <enable_wind>0</enable_wind>
  1976.         <kinematic>0</kinematic>
  1977.         <gravity>0</gravity>
  1978.       </link>
  1979.       <pose frame=''>11.0983 1.98454 0 0 -0 0</pose>
  1980.     </model>
  1981.  
  1982.     <model name='target_green'>
  1983.       <static>0</static>
  1984.       <link name='link'>
  1985.         <pose frame=''>0 0 0 0 0 0</pose>
  1986.         <collision name='Target_Green_Collision'>
  1987.             <geometry>
  1988.               <mesh>
  1989.                 <uri>model://target_green/meshes/target_green.dae</uri>
  1990.                 <scale>0.9 0.9 0.9</scale>
  1991.               </mesh>
  1992.             </geometry>
  1993.             <max_contacts>10</max_contacts>
  1994.             <surface>
  1995.               <contact>
  1996.                 <ode/>
  1997.               </contact>
  1998.               <bounce/>
  1999.               <friction>
  2000.                 <torsional>
  2001.                   <ode/>
  2002.                 </torsional>
  2003.                 <ode/>
  2004.               </friction>
  2005.             </surface>
  2006.             <pose frame=''>0 0 -0.225 0 0 3.1415</pose>
  2007.         </collision>
  2008.         <visual name='visual'>
  2009.           <geometry>
  2010.             <mesh>
  2011.               <uri>model://target_green/meshes/target_green.dae</uri>
  2012.               <scale>0.9 0.9 0.9</scale>
  2013.             </mesh>
  2014.           </geometry>
  2015.           <pose frame=''>0 0 -0.225 0 0 3.1415</pose>
  2016.         </visual>
  2017.         <self_collide>0</self_collide>
  2018.         <enable_wind>0</enable_wind>
  2019.         <kinematic>0</kinematic>
  2020.         <gravity>0</gravity>
  2021.       </link>
  2022.       <pose frame=''>15.6869 -7.2778 0 0 -0 0</pose>
  2023.     </model>
  2024.   </world>
  2025. </sdf>

map_maze_target.launch

  1. <?xml version="1.0"?>
  2. <launch>
  3.     <!-- MAVROS posix SITL environment launch script -->
  4.     <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
  5.     <!-- vehicle model and world -->
  6.     <arg name="est" default="ekf2"/>
  7.     <arg name="world" default="/home/xtdrone/user_ws/run/uav_env/map_maze_target.world"/>
  8.     <!-- gazebo configs -->
  9.     <arg name="gui" default="true"/>
  10.     <arg name="debug" default="false"/>
  11.     <arg name="verbose" default="false"/>
  12.     <arg name="paused" default="false"/>
  13.     <!-- Gazebo sim -->
  14.     <include file="$(find gazebo_ros)/launch/empty_world.launch">
  15.         <arg name="gui" value="$(arg gui)"/>
  16.         <arg name="world_name" value="$(arg world)"/>
  17.         <arg name="debug" value="$(arg debug)"/>
  18.         <arg name="verbose" value="$(arg verbose)"/>
  19.         <arg name="paused" value="$(arg paused)"/>
  20.     </include>
  21.      <!-- iris_0 -->
  22.      <group ns="iris_0">
  23.         <!-- MAVROS and vehicle configs -->
  24.             <arg name="ID" value="0"/>
  25.             <arg name="ID_in_group" value="0"/>
  26.             <arg name="fcu_url" default="udp://:24540@localhost:34580"/>
  27.         <!-- PX4 SITL and vehicle spawn -->
  28.         <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
  29.             <arg name="x" value="-16"/> <!-- -16 for map2-1, -18 for map2-2, -18 for map2-3 -->
  30.             <arg name="y" value="0"/>
  31.             <arg name="z" value="0.5"/>
  32.             <arg name="R" value="0"/>
  33.             <arg name="P" value="0"/>
  34.             <arg name="Y" value="0"/>
  35.             <arg name="vehicle" value="iris"/>
  36.             <arg name="sdf" value="iris_all_sensor"/>
  37.             <arg name="mavlink_udp_port" value="18570"/>
  38.             <arg name="mavlink_tcp_port" value="4560"/>
  39.             <arg name="ID" value="$(arg ID)"/>
  40.             <arg name="ID_in_group" value="$(arg ID_in_group)"/>
  41.         </include>
  42.         <!-- MAVROS -->
  43.         <include file="$(find mavros)/launch/px4.launch">
  44.             <arg name="fcu_url" value="$(arg fcu_url)"/>
  45.             <arg name="gcs_url" value=""/>
  46.             <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
  47.             <arg name="tgt_component" value="1"/>
  48.         </include>
  49.     </group>
  50. </launch>
  51. <!--the launch file is generated by XTDrone multi-vehicle generator.py  -->

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小小林熬夜学编程/article/detail/453487
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号