.. _sec_gazebo_sensor_lab:

综合实验：基于Gazebo的虚拟模块组装
==================================


   **学习目标**

   - 掌握Gazebo仿真环境中传感器模型的SDF配置方法
   - 理解真实硬件参数到仿真参数的映射关系
   - 学会使用ROS工具订阅、可视化和分析传感器数据
   - 完成多传感器集成与坐标系标定

本章通过四个递进式实验，引导读者在Gazebo仿真环境中完成传感器的建模、配置和集成。每个实验以真实硬件规格为原型，帮助读者建立仿真与实物之间的对应关系。

.. _sec_lab_mono_camera:

实验一：单目相机仿真
--------------------


实验目的与任务
~~~~~~~~~~~~~~

   **实验目的**

   - 理解单目相机的成像原理和核心参数（分辨率、帧率、视场角FOV）
   - 掌握Gazebo仿真环境中相机传感器的SDF配置方法
   - 学会将真实硬件规格书参数映射到仿真模型参数
   - 掌握在ROS中订阅图像话题并进行可视化的方法

**实验任务**\ ：

1. 阅读USB单目相机（DECXIN-OG2M-1888V1）规格书，提取关键参数
2. 根据硬件参数创建Gazebo相机SDF模型文件
3. 在Gazebo中加载相机模型并验证ROS话题输出
4. 使用\ ``rqt_image_view``\ 和rviz可视化相机图像
5. 完成参数调整实验，观察不同参数对图像的影响

实验预习
~~~~~~~~

**预备知识**\ ：

1. **针孔相机模型**\ ：了解相机成像的基本原理，理解焦距、像素尺寸、视场角之间的关系
2. **ROS基础**\ ：熟悉ROS话题（Topic）的发布与订阅机制，了解\ ``sensor_msgs/Image``\ 消息类型
3. **SDF/URDF格式**\ ：了解Gazebo模型描述文件的基本结构
4. **Linux基础**\ ：熟悉基本的终端命令操作

**课前思考题**\ ：

1. 什么是相机的视场角（FOV）？对角线FOV与水平FOV如何换算？
2. 相机的分辨率和帧率之间通常存在什么样的权衡关系？
3. Gazebo中的相机插件如何将图像数据发布到ROS话题？

..

   **安全提示**

   本实验为纯软件仿真实验，无人身安全隐患。但请注意：高分辨率、高帧率的相机仿真会消耗大量GPU资源，可能导致系统卡顿。建议在仿真中使用降采样的分辨率（如640×480）以保证流畅运行。

实验原理
~~~~~~~~

针孔相机成像模型
^^^^^^^^^^^^^^^^

单目相机的成像过程可以用\ *针孔相机模型*\ （Pinhole Camera
Model）描述。三维空间中的点 :math:`P(X, Y, Z)` 通过光心 :math:`O`
投影到像素平面上的点 :math:`p(u, v)`\ ，其投影关系为：

.. math:: \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \frac{1}{Z} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix}.
   :label: eq_pinhole_model

其中：

- :math:`(f_x, f_y)` 是以像素为单位的焦距
- :math:`(c_x, c_y)` 是光心在像素坐标系中的位置
- :math:`Z` 是点到相机的深度

视场角（FOV）计算
^^^^^^^^^^^^^^^^^

*视场角*\ （FOV, Field of
View）决定了相机能够”看到”的范围。已知对角线FOV和图像宽高比，可计算水平FOV：

.. math:: \text{FOV}_{horizontal} = 2 \times \arctan\left(\frac{w}{\sqrt{w^2 + h^2}} \times \tan\left(\frac{\text{FOV}_{diagonal}}{2}\right)\right).
   :label: eq_fov_conversion

对于本实验使用的USB相机（1600×1300分辨率，92°对角线FOV）：

- 宽高比：\ :math:`w:h = 1600:1300 \approx 1.23:1`
- 计算得水平FOV ≈ 77° ≈ 1.34 rad

硬件参数到仿真参数的映射
^^^^^^^^^^^^^^^^^^^^^^^^

=============== ======================== =====================
硬件参数        SDF参数                  映射方法
=============== ======================== =====================
分辨率1600×1300 ``<width>`` ``<height>`` 直接映射或降采样
帧率60fps       ``<update_rate>``        直接映射
对角线FOV 92°   ``<horizontal_fov>``     转换为水平FOV（弧度）
像素尺寸3.0μm   影响噪声模型             间接映射
=============== ======================== =====================

Gazebo相机插件工作原理
^^^^^^^^^^^^^^^^^^^^^^

Gazebo通过\ ``libgazebo_ros_camera.so``\ 插件将仿真相机的图像数据发布到ROS话题。插件的主要功能包括：

1. 渲染虚拟场景生成图像
2. 添加噪声模型模拟真实相机特性
3. 将图像封装为ROS消息并发布
4. 发布相机内参信息（\ ``camera_info``\ 话题）

实验内容
~~~~~~~~

硬件原型参数表
^^^^^^^^^^^^^^

本实验以深圳德创信USB摄像模组（型号：DECXIN-OG2M-1888V1）为原型：

=========== ============= ====================
参数        规格值        说明
=========== ============= ====================
感光芯片    1/2.9”        芯片尺寸
最高分辨率  1600×1300     约200万像素
支持帧率    60fps @ MJPEG 压缩格式下的最高帧率
视场角(FOV) 92°（对角线） 广角镜头
焦距        2.87mm        定焦镜头
光圈        F2.2          大光圈设计
像素尺寸    3.0μm × 3.0μm 单个像素物理尺寸
接口        USB2.0        即插即用
尺寸        38×38×18mm    紧凑设计
工作电流    80-120mA      低功耗
=========== ============= ====================

创建模型目录结构
^^^^^^^^^^^^^^^^

.. raw:: latex

   \diilbookstyleinputcell

.. code:: bash

   # 步骤1：创建模型目录
   mkdir -p ~/catkin_ws/src/my_sensors/models/mono_camera
   cd ~/catkin_ws/src/my_sensors/models/mono_camera

创建model.config文件
^^^^^^^^^^^^^^^^^^^^

.. raw:: latex

   \diilbookstyleinputcell

.. code:: xml

   <?xml version="1.0"?>
   <model>
     <name>mono_camera</name>
     <version>1.0</version>
     <sdf version="1.6">model.sdf</sdf>
     <author>
       <name>Student</name>
       <email>student@example.com</email>
     </author>
     <description>
       基于DECXIN-OG2M-1888V1参数的单目相机模型
     </description>
   </model>

创建model.sdf文件
^^^^^^^^^^^^^^^^^

.. raw:: latex

   \diilbookstyleinputcell

.. code:: xml

   <?xml version="1.0"?>
   <sdf version="1.6">
     <model name="mono_camera">
       <!-- 相机外壳 -->
       <link name="camera_link">
         <pose>0 0 0 0 0 0</pose>
         
         <!-- 惯性参数 -->
         <inertial>
           <mass>0.050</mass>
           <inertia>
             <ixx>0.00001</ixx>
             <iyy>0.00001</iyy>
             <izz>0.00001</izz>
           </inertia>
         </inertial>
         
         <!-- 视觉外观 -->
         <visual name="camera_visual">
           <geometry>
             <box>
               <size>0.038 0.038 0.018</size>
             </box>
           </geometry>
           <material>
             <ambient>0.1 0.1 0.1 1</ambient>
             <diffuse>0.2 0.2 0.2 1</diffuse>
           </material>
         </visual>
         
         <!-- 碰撞体积 -->
         <collision name="camera_collision">
           <geometry>
             <box>
               <size>0.038 0.038 0.018</size>
             </box>
           </geometry>
         </collision>
         
         <!-- 相机传感器 -->
         <sensor name="mono_camera_sensor" type="camera">
           <pose>0.019 0 0 0 0 0</pose>
           <always_on>true</always_on>
           <update_rate>30</update_rate>
           
           <camera name="mono_cam">
             <horizontal_fov>1.34</horizontal_fov>
             <image>
               <width>640</width>
               <height>480</height>
               <format>R8G8B8</format>
             </image>
             <clip>
               <near>0.1</near>
               <far>100</far>
             </clip>
             <noise>
               <type>gaussian</type>
               <mean>0.0</mean>
               <stddev>0.007</stddev>
             </noise>
           </camera>
           
           <!-- ROS插件 -->
           <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
             <alwaysOn>true</alwaysOn>
             <updateRate>30.0</updateRate>
             <cameraName>mono_camera</cameraName>
             <imageTopicName>image_raw</imageTopicName>
             <cameraInfoTopicName>camera_info</cameraInfoTopicName>
             <frameName>camera_link</frameName>
           </plugin>
         </sensor>
       </link>
     </model>
   </sdf>

运行仿真
^^^^^^^^

**步骤1：启动Gazebo**

.. raw:: latex

   \diilbookstyleinputcell

.. code:: bash

   roslaunch gazebo_ros empty_world.launch

**步骤2：设置模型路径并加载模型**

.. raw:: latex

   \diilbookstyleinputcell

.. code:: bash

   export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/catkin_ws/src/my_sensors/models
   rosrun gazebo_ros spawn_model -sdf -file ~/catkin_ws/src/my_sensors/models/mono_camera/model.sdf -model mono_camera -x 0 -y 0 -z 1

**步骤3：验证ROS话题**

.. raw:: latex

   \diilbookstyleinputcell

.. code:: bash

   rostopic list | grep mono_camera
   # 预期输出：
   # /mono_camera/camera_info
   # /mono_camera/image_raw

**步骤4：可视化图像**

.. raw:: latex

   \diilbookstyleinputcell

.. code:: bash

   rqt_image_view /mono_camera/image_raw

参数调整实验
^^^^^^^^^^^^

修改SDF文件中的参数，重新加载模型，观察效果变化：

====== ==================== ===== ==== ============
实验项 修改参数             原值  新值 观察内容
====== ==================== ===== ==== ============
实验A  ``<horizontal_fov>`` 1.34  0.8  视野范围变窄
实验B  ``<update_rate>``    30    10   图像更新变慢
实验C  ``<stddev>``         0.007 0.05 图像噪点增加
====== ==================== ===== ==== ============

实验报告要求
~~~~~~~~~~~~

1. **模型配置记录**\ ：记录创建的SDF文件内容，标注各参数的含义
2. **话题验证截图**\ ：提供\ ``rostopic list``\ 和\ ``rostopic hz``\ 命令的输出截图
3. **图像可视化截图**\ ：提供\ ``rqt_image_view``\ 显示图像的截图
4. **参数调整对比**\ ：完成参数调整实验，记录观察结果并分析原因
5. **相机内参分析**\ ：使用\ ``rostopic echo /mono_camera/camera_info -n 1``\ 获取相机内参，解释K矩阵各元素的含义

思考题
~~~~~~

1. 为什么仿真中将分辨率从1600×1300降采样到640×480？这样做有什么优缺点？

2. 相机的视场角（FOV）对无人机的避障应用有什么影响？FOV越大越好吗？

3. 查阅资料，说明Gazebo相机噪声模型中的高斯噪声标准差应该如何根据真实相机的信噪比（SNR）来设置？

4. 如果需要在仿真中模拟相机的径向畸变，应该如何修改SDF配置？

.. _sec_lab_depth_camera:

.. _实验目的与任务-1:

实验二：双目深度相机仿真
------------------------



.. _实验预习-1:

实验目的与任务
~~~~~~~~~~~~~~

   **实验目的**

   - 理解双目立体视觉的深度测量原理
   - 掌握Intel RealSense D435i深度相机的核心参数和工作特性
   - 学会配置Gazebo深度相机插件，获取RGB图像、深度图像和点云数据
   - 理解深度相机的测量范围与精度限制

**实验任务**\ ：

1. 学习D435i硬件规格，理解主动立体视觉深度测量原理
2. 创建D435i的Gazebo仿真模型（含深度相机、RGB相机、IMU）
3. 在Gazebo中加载模型并验证多个ROS话题的输出
4. 使用rviz可视化深度图像和点云
5. 编写Python脚本进行深度测量和点云分析


.. _实验原理-1:

实验预习
~~~~~~~~

**预备知识**\ ：

1. **立体视觉原理**\ ：理解\ *视差*\ （disparity）与深度的关系
2. **点云数据结构**\ ：了解\ ``sensor_msgs/PointCloud2``\ 消息格式
3. **ROS图像处理**\ ：熟悉\ ``cv_bridge``\ 库的使用方法
4. **坐标系变换**\ ：理解相机坐标系与世界坐标系的关系

**课前思考题**\ ：

1. 双目相机的基线（baseline）对深度测量精度有什么影响？
2. D435i为什么要使用红外结构光投射器？纯双目相机有什么局限性？
3. 深度相机在什么情况下会产生无效深度值？

..

   **安全提示**

   深度相机仿真涉及点云渲染，对GPU负载较高，建议关闭不必要的后台程序。真实D435i使用红外光，避免将相机对准眼睛长时间观看。


.. _实验内容-1:

实验原理
~~~~~~~~

双目立体视觉深度测量原理
^^^^^^^^^^^^^^^^^^^^^^^^

D435i使用\ *主动立体视觉*\ 技术测量深度。其基本原理是三角测量：

.. math:: Z = \frac{f \times B}{d}.
   :label: eq_stereo_depth

其中：

- :math:`Z` 是深度（目标到相机的距离）
- :math:`f` 是焦距（像素单位）
- :math:`B` 是基线距离（D435i为50mm）
- :math:`d` 是视差（左右图像中对应点的像素差）

深度精度分析
^^^^^^^^^^^^

深度测量误差与距离的平方成正比：

.. math:: \Delta Z = \frac{Z^2}{f \times B} \Delta d.
   :label: eq_depth_error

这解释了为什么深度相机在远距离时精度下降。对于D435i（基线50mm），在不同距离的理论精度：

==== ========
距离 理论精度
==== ========
1m   ±2mm
2m   ±8mm
5m   ±50mm
10m  ±200mm
==== ========

D435i传感器组成
^^^^^^^^^^^^^^^

D435i集成了多个传感器模块：

::

   D435i内部结构
   ├── 左红外相机 ─┐
   ├── 右红外相机 ─┼─→ 深度计算引擎 → 深度图/点云
   ├── 红外投射器 ─┘
   ├── RGB彩色相机 → 彩色图像
   └── IMU (BMI055) → 加速度/角速度


.. _实验报告要求-1:

实验内容
~~~~~~~~

D435i硬件规格
^^^^^^^^^^^^^

========== ============ ================
参数       规格值       说明
========== ============ ================
深度技术   主动立体视觉 红外结构光辅助
深度分辨率 最高1280×720 可配置
深度帧率   最高90fps    取决于分辨率
RGB分辨率  1920×1080    全高清
深度FOV    87°×58°      水平×垂直
测距范围   0.1m-10m     推荐0.3m-3m
基线       50mm         左右红外相机间距
IMU        BMI055       6轴惯性测量单元
接口       USB 3.0      高速数据传输
尺寸       90×25×25mm   紧凑设计
重量       72g          轻量化
========== ============ ================

创建D435i模型
^^^^^^^^^^^^^

创建\ ``model.sdf``\ 文件（深度相机部分）：

.. raw:: latex

   \diilbookstyleinputcell

.. code:: xml

   <?xml version="1.0"?>
   <sdf version="1.6">
     <model name="d435i">
       <link name="d435i_link">
         <pose>0 0 0 0 0 0</pose>
         
         <inertial>
           <mass>0.072</mass>
           <inertia>
             <ixx>0.0001</ixx>
             <iyy>0.0001</iyy>
             <izz>0.0001</izz>
           </inertia>
         </inertial>
         
         <visual name="d435i_visual">
           <geometry>
             <box>
               <size>0.090 0.025 0.025</size>
             </box>
           </geometry>
           <material>
             <ambient>0.5 0.5 0.5 1</ambient>
           </material>
         </visual>
         
         <!-- 深度相机传感器 -->
         <sensor name="depth_camera" type="depth">
           <pose>0 0 0 0 0 0</pose>
           <always_on>true</always_on>
           <update_rate>30</update_rate>
           
           <camera name="depth_cam">
             <horizontal_fov>1.52</horizontal_fov>
             <image>
               <width>640</width>
               <height>480</height>
               <format>R8G8B8</format>
             </image>
             <clip>
               <near>0.1</near>
               <far>10</far>
             </clip>
           </camera>
           
           <plugin name="depth_camera_controller" filename="libgazebo_ros_openni_kinect.so">
             <alwaysOn>true</alwaysOn>
             <updateRate>30.0</updateRate>
             <cameraName>d435i</cameraName>
             <imageTopicName>color/image_raw</imageTopicName>
             <depthImageTopicName>depth/image_raw</depthImageTopicName>
             <pointCloudTopicName>depth/points</pointCloudTopicName>
             <cameraInfoTopicName>color/camera_info</cameraInfoTopicName>
             <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
             <frameName>d435i_link</frameName>
             <pointCloudCutoff>0.1</pointCloudCutoff>
             <pointCloudCutoffMax>10.0</pointCloudCutoffMax>
           </plugin>
         </sensor>
       </link>
     </model>
   </sdf>

深度数据分析脚本
^^^^^^^^^^^^^^^^

创建\ ``depth_analysis.py``\ ：

.. raw:: latex

   \diilbookstyleinputcell

.. code:: python

   #!/usr/bin/env python
   # -*- coding: utf-8 -*-
   # 文件名: depth_analysis.py

   import rospy
   import numpy as np
   from sensor_msgs.msg import Image
   from cv_bridge import CvBridge

   class DepthAnalysis:
       def __init__(self):
           rospy.init_node('depth_analysis')
           self.bridge = CvBridge()
           self.sub = rospy.Subscriber('/d435i/depth/image_raw', 
                                       Image, self.depth_callback)
           rospy.loginfo("深度分析节点已启动")
           
       def depth_callback(self, msg):
           # 将ROS图像消息转换为numpy数组
           depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='32FC1')
           
           # 统计有效深度值
           valid_mask = np.isfinite(depth_image) & (depth_image > 0)
           valid_depths = depth_image[valid_mask]
           
           if len(valid_depths) > 0:
               rospy.loginfo("深度统计: 最小=%.2fm, 最大=%.2fm, 平均=%.2fm" % 
                            (valid_depths.min(), valid_depths.max(), valid_depths.mean()))

   if __name__ == '__main__':
       da = DepthAnalysis()
       rospy.spin()


.. _思考题-1:

实验报告要求
~~~~~~~~~~~~

1. **深度图可视化**\ ：使用rviz显示深度图像，调整颜色映射
2. **点云可视化**\ ：在rviz中显示点云，尝试不同的点大小和颜色模式
3. **深度精度测试**\ ：放置已知距离的物体，对比深度测量值与实际值
4. **有效范围验证**\ ：测试不同距离下深度值的有效性


思考题
~~~~~~

1. D435i的基线为50mm，如果增大基线到100mm，对深度测量有什么影响？

2. 深度相机在遇到透明物体（玻璃）、镜面反射、黑色物体时会出现什么问题？

3. 比较D435i与TOF深度相机（如Azure Kinect）的优缺点。

.. _sec_lab_lidar:

.. _实验目的与任务-2:

实验三：激光雷达仿真
--------------------



.. _实验原理-2:

实验目的与任务
~~~~~~~~~~~~~~

   **实验目的**

   - 理解激光雷达的测距原理和扫描方式
   - 掌握Livox Mid-360的核心特性（非重复扫描、FOV覆盖）
   - 学会配置Gazebo激光雷达插件并获取点云数据
   - 理解点云数据的结构和处理方法

**实验任务**\ ：

1. 学习Mid-360硬件规格，理解其非重复扫描特性
2. 创建Mid-360的Gazebo仿真模型
3. 在Gazebo中验证点云输出和FOV覆盖
4. 编写点云分析脚本


.. _实验内容-2:

实验原理
~~~~~~~~

激光雷达测距原理
^^^^^^^^^^^^^^^^

激光雷达通过测量激光脉冲的飞行时间（\ *TOF*, Time of
Flight）来计算距离：

.. math:: d = \frac{c \cdot \Delta t}{2}.
   :label: eq_lidar_tof

其中 :math:`c` 是光速（约3×10⁸ m/s），\ :math:`\Delta t`
是激光往返时间。

Mid-360非重复扫描特性
^^^^^^^^^^^^^^^^^^^^^

与传统机械式激光雷达不同，Mid-360采用\ *非重复扫描*\ （Non-repetitive
Scanning）技术：

- 扫描路径在短时间内不重复
- 随时间累积，FOV内覆盖率逐渐提高
- 0.1秒覆盖率约80%，0.5秒接近100%

Mid-360技术规格
^^^^^^^^^^^^^^^

======== ============ =============
参数     规格值       说明
======== ============ =============
扫描方式 非重复扫描   Livox专利技术
水平FOV  360°         全向覆盖
垂直FOV  -7°~+52°     非对称设计
测距范围 0.1m-40m     @10%反射率
测距精度 <2cm         @10m
点云速率 200,000点/秒 高密度
激光波长 905nm        人眼安全
重量     265g         适合无人机
功耗     6.5W         典型值
======== ============ =============


.. _思考题-2:

实验内容
~~~~~~~~

创建Mid-360模型
^^^^^^^^^^^^^^^

.. raw:: latex

   \diilbookstyleinputcell

.. code:: xml

   <?xml version="1.0"?>
   <sdf version="1.6">
     <model name="mid360">
       <link name="mid360_link">
         <pose>0 0 0 0 0 0</pose>
         
         <inertial>
           <mass>0.265</mass>
           <inertia>
             <ixx>0.0005</ixx>
             <iyy>0.0005</iyy>
             <izz>0.0005</izz>
           </inertia>
         </inertial>
         
         <visual name="mid360_visual">
           <geometry>
             <cylinder>
               <radius>0.0325</radius>
               <length>0.060</length>
             </cylinder>
           </geometry>
           <material>
             <ambient>0.3 0.3 0.3 1</ambient>
           </material>
         </visual>
         
         <!-- 激光雷达传感器 -->
         <sensor name="lidar" type="gpu_ray">
           <pose>0 0 0.03 0 0 0</pose>
           <always_on>true</always_on>
           <update_rate>10</update_rate>
           <visualize>true</visualize>
           
           <ray>
             <scan>
               <horizontal>
                 <samples>360</samples>
                 <resolution>1</resolution>
                 <min_angle>-3.14159</min_angle>
                 <max_angle>3.14159</max_angle>
               </horizontal>
               <vertical>
                 <samples>60</samples>
                 <resolution>1</resolution>
                 <min_angle>-0.122</min_angle>
                 <max_angle>0.908</max_angle>
               </vertical>
             </scan>
             <range>
               <min>0.1</min>
               <max>40</max>
               <resolution>0.01</resolution>
             </range>
             <noise>
               <type>gaussian</type>
               <mean>0</mean>
               <stddev>0.02</stddev>
             </noise>
           </ray>
           
           <plugin name="lidar_controller" filename="libgazebo_ros_velodyne_gpu_laser.so">
             <topicName>/mid360/points</topicName>
             <frameName>mid360_link</frameName>
             <organize_cloud>false</organize_cloud>
             <min_range>0.1</min_range>
             <max_range>40.0</max_range>
           </plugin>
         </sensor>
       </link>
     </model>
   </sdf>

点云分析脚本
^^^^^^^^^^^^

创建\ ``lidar_analysis.py``\ ：

.. raw:: latex

   \diilbookstyleinputcell

.. code:: python

   #!/usr/bin/env python
   # -*- coding: utf-8 -*-
   # 文件名: lidar_analysis.py

   import rospy
   import numpy as np
   from sensor_msgs.msg import PointCloud2
   import sensor_msgs.point_cloud2 as pc2

   class LidarAnalysis:
       def __init__(self):
           rospy.init_node('lidar_analysis')
           self.sub = rospy.Subscriber('/mid360/points', 
                                       PointCloud2, self.cloud_callback)
           rospy.loginfo("激光雷达分析节点已启动")
           
       def cloud_callback(self, msg):
           # 将PointCloud2转换为numpy数组
           points = np.array(list(pc2.read_points(msg, 
                            field_names=("x", "y", "z"), skip_nans=True)))
           
           if len(points) > 0:
               rospy.loginfo("点云数量: %d" % len(points))
               rospy.loginfo("X范围: [%.1f, %.1f]" % (points[:,0].min(), points[:,0].max()))
               rospy.loginfo("Y范围: [%.1f, %.1f]" % (points[:,1].min(), points[:,1].max()))
               rospy.loginfo("Z范围: [%.1f, %.1f]" % (points[:,2].min(), points[:,2].max()))

   if __name__ == '__main__':
       la = LidarAnalysis()
       rospy.spin()


思考题
~~~~~~

1. Gazebo中使用的激光雷达插件是基于传统多线扫描模型的，无法真正模拟Mid-360的非重复扫描特性。你认为应该如何改进仿真模型来更准确地模拟这一特性？

2. Mid-360的垂直FOV为-7°~+52°，如果将其安装在无人机顶部，这个FOV分布对于不同应用场景（如室内导航、室外建图）有什么影响？

3. 激光雷达在遇到玻璃、镜面、黑色物体时会出现什么问题？为什么？

4. Mid-360的点云速率为200,000点/秒，帧率为10Hz，这意味着每帧平均有多少个点？如果用于实时SLAM算法，这个数据量是否需要进行下采样？

.. _sec_lab_sensor_integration:

.. _实验目的与任务-3:

实验四：多传感器集成
--------------------



.. _实验原理-3:

实验目的与任务
~~~~~~~~~~~~~~

   **实验目的**

   - 掌握将多个传感器集成到无人机模型的方法
   - 理解传感器坐标系与机体坐标系的变换关系
   - 学会使用TF（Transform）管理多传感器坐标系
   - 实现多传感器数据的时间同步

**实验任务**\ ：

1. 设计传感器在无人机上的布局方案
2. 创建集成多传感器的URDF/Xacro模型
3. 验证TF坐标系关系的正确性
4. 实现多传感器数据的同步接收
5. 构建完整的智能无人机感知系统


.. _实验内容-3:

实验原理
~~~~~~~~

传感器坐标系定义
^^^^^^^^^^^^^^^^

每个传感器都有自己的坐标系，需要通过\ *TF变换*\ 统一到机体坐标系：

::

   world (世界坐标系)
     └── base_link (机体坐标系)
           ├── mono_camera_link (单目相机)
           ├── d435i_link (深度相机)
           └── mid360_link (激光雷达)

坐标变换表示
^^^^^^^^^^^^

从传感器坐标系到机体坐标系的变换包括：

- **平移** (x, y, z)：传感器相对机体中心的位置
- **旋转** (roll, pitch, yaw)：传感器的安装角度

变换矩阵表示为：

.. math:: T_{body}^{sensor} = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}.
   :label: eq_transform_matrix

时间同步机制
^^^^^^^^^^^^

ROS中使用\ ``message_filters``\ 实现多话题的时间同步：

- **ExactTime**\ ：要求时间戳完全相同
- **ApproximateTime**\ ：允许一定的时间差（slop）

传感器布局设计原则
^^^^^^^^^^^^^^^^^^

================ ==================================
原则             说明
================ ==================================
**FOV不遮挡**    传感器之间不能互相遮挡视野
**重心平衡**     传感器布局应尽量保持无人机重心稳定
**减少振动影响** IMU等敏感传感器应远离电机
**便于维护**     传感器应便于安装和拆卸
================ ==================================


.. _实验报告要求-2:

实验内容
~~~~~~~~

传感器布局方案
^^^^^^^^^^^^^^

P450无人机上的传感器布局设计：

======== ============ =========== ============
传感器   位置         安装角度    用途
======== ============ =========== ============
单目相机 机头前方15cm 向下倾斜15° 前视避障
D435i    机身下方8cm  向下倾斜30° 深度感知
Mid-360  机身顶部8cm  水平安装    360°环境感知
======== ============ =========== ============

创建集成模型
^^^^^^^^^^^^

创建\ ``p450_sensors.urdf.xacro``\ 文件：

.. raw:: latex

   \diilbookstyleinputcell

.. code:: xml

   <?xml version="1.0"?>
   <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="p450_sensors">
     
     <!-- 单目相机 -->
     <link name="mono_camera_link">
       <visual>
         <geometry><box size="0.038 0.038 0.018"/></geometry>
         <material name="dark_grey"><color rgba="0.2 0.2 0.2 1"/></material>
       </visual>
       <collision>
         <geometry><box size="0.038 0.038 0.018"/></geometry>
       </collision>
       <inertial>
         <mass value="0.05"/>
         <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
       </inertial>
     </link>
     
     <joint name="mono_camera_joint" type="fixed">
       <parent link="base_link"/>
       <child link="mono_camera_link"/>
       <origin xyz="0.15 0 -0.05" rpy="0 0.26 0"/>
     </joint>
     
     <!-- D435i -->
     <link name="d435i_link">
       <visual>
         <geometry><box size="0.090 0.025 0.025"/></geometry>
         <material name="grey"><color rgba="0.7 0.7 0.7 1"/></material>
       </visual>
       <collision>
         <geometry><box size="0.090 0.025 0.025"/></geometry>
       </collision>
       <inertial>
         <mass value="0.072"/>
         <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
       </inertial>
     </link>
     
     <joint name="d435i_joint" type="fixed">
       <parent link="base_link"/>
       <child link="d435i_link"/>
       <origin xyz="0.05 0 -0.08" rpy="0 0.52 0"/>
     </joint>
     
     <!-- Mid-360 -->
     <link name="mid360_link">
       <visual>
         <geometry><cylinder radius="0.0325" length="0.06"/></geometry>
         <material name="dark_grey"><color rgba="0.3 0.3 0.3 1"/></material>
       </visual>
       <collision>
         <geometry><box size="0.065 0.065 0.060"/></geometry>
       </collision>
       <inertial>
         <mass value="0.265"/>
         <inertia ixx="0.0005" ixy="0" ixz="0" iyy="0.0005" iyz="0" izz="0.0005"/>
       </inertial>
     </link>
     
     <joint name="mid360_joint" type="fixed">
       <parent link="base_link"/>
       <child link="mid360_link"/>
       <origin xyz="0 0 0.08" rpy="0 0 0"/>
     </joint>
     
   </robot>

多传感器时间同步
^^^^^^^^^^^^^^^^

创建\ ``sensor_sync.py``\ ：

.. raw:: latex

   \diilbookstyleinputcell

.. code:: python

   #!/usr/bin/env python
   # -*- coding: utf-8 -*-
   # 文件名: sensor_sync.py

   import rospy
   import message_filters
   from sensor_msgs.msg import Image, PointCloud2

   class SensorSync:
       def __init__(self):
           rospy.init_node('sensor_sync')
           
           mono_sub = message_filters.Subscriber('/mono_camera/image_raw', Image)
           depth_sub = message_filters.Subscriber('/d435i/depth/image_raw', Image)
           lidar_sub = message_filters.Subscriber('/mid360/points', PointCloud2)
           
           ts = message_filters.ApproximateTimeSynchronizer(
               [mono_sub, depth_sub, lidar_sub], 
               queue_size=10, 
               slop=0.1
           )
           ts.registerCallback(self.callback)
           rospy.loginfo("传感器同步节点已启动")
           
       def callback(self, mono_img, depth_img, lidar_points):
           mono_time = mono_img.header.stamp.to_sec()
           depth_time = depth_img.header.stamp.to_sec()
           lidar_time = lidar_points.header.stamp.to_sec()
           
           time_diff = max(mono_time, depth_time, lidar_time) - min(mono_time, depth_time, lidar_time)
           rospy.loginfo("同步成功! 最大时间差: %.3fs" % time_diff)

   if __name__ == '__main__':
       ss = SensorSync()
       rospy.spin()

TF坐标系验证
^^^^^^^^^^^^

.. raw:: latex

   \diilbookstyleinputcell

.. code:: bash

   # 查看TF树
   rosrun rqt_tf_tree rqt_tf_tree

   # 检查特定变换
   rosrun tf tf_echo base_link mono_camera_link
   rosrun tf tf_echo base_link d435i_link
   rosrun tf tf_echo base_link mid360_link


.. _思考题-3:

实验报告要求
~~~~~~~~~~~~

1. **传感器布局图**\ ：绘制无人机俯视图和侧视图，标注各传感器的安装位置和角度
2. **TF树截图**\ ：提供\ ``rqt_tf_tree``\ 显示的完整TF树截图
3. **坐标变换验证**\ ：使用\ ``tf_echo``\ 验证各传感器到\ ``base_link``\ 的变换，记录输出结果
4. **时间同步测试**\ ：运行\ ``sensor_sync.py``\ ，记录同步成功率和时间差统计


思考题
~~~~~~

1. 如果单目相机和D435i的视野有重叠区域，这是好事还是坏事？在什么应用场景下需要这种设计？

2. Mid-360安装在机身顶部，会被无人机的桨叶遮挡一部分FOV。如何评估这种遮挡的影响？有什么解决方案？

3. 时间同步中使用的\ ``slop=0.1``\ 意味着允许100ms的时间差。对于高速飞行的无人机（如10m/s），100ms内会移动多少距离？这个误差是否可以接受？

4. 如果要将仿真中的传感器配置迁移到真实无人机上，需要额外考虑哪些因素？（提示：标定、EMI、供电等）

附录
----

附录A：硬件规格对比表
~~~~~~~~~~~~~~~~~~~~~

============ =========== =============== =============
参数         USB单目相机 Intel D435i     Livox Mid-360
============ =========== =============== =============
**类型**     2D视觉      深度视觉        3D激光
**分辨率**   1600×1300   1280×720 (深度) 200k点/秒
**帧率**     60fps       30fps           10Hz
**FOV**      92°(对角)   87°×58°         360°×59°
**测量范围** N/A         0.1-10m         0.1-40m
**精度**     N/A         ±2% @ 2m        <2cm @ 10m
**接口**     USB2.0      USB3.0          以太网
**功耗**     0.5W        2.5W            6.5W
**重量**     50g         72g             265g
**价格参考** ￥50-100    ￥2000-3000     ￥4000-5000
============ =========== =============== =============

附录B：常见问题与解决方案
~~~~~~~~~~~~~~~~~~~~~~~~~

+----------------------------+-----------------------+----------------------------------------------+
| 问题                       | 可能原因              | 解决方案                                     |
+============================+=======================+==============================================+
| Gazebo中看不到传感器可视化 | visualize未开启       | 在SDF中设置\ ``<visualize>true</visualize>`` |
+----------------------------+-----------------------+----------------------------------------------+
| 点云话题无数据             | 插件未正确加载        | 检查plugin路径和名称                         |
+----------------------------+-----------------------+----------------------------------------------+
| 图像话题延迟高             | 分辨率/帧率过高       | 降低分辨率或帧率                             |
+----------------------------+-----------------------+----------------------------------------------+
| TF坐标系错误               | joint配置错误         | 使用\ ``tf_echo``\ 检查变换                  |
+----------------------------+-----------------------+----------------------------------------------+
| 仿真卡顿                   | GPU负载过高           | 减少同时运行的传感器数量                     |
+----------------------------+-----------------------+----------------------------------------------+

延伸阅读
--------

- `Gazebo传感器教程 <http://gazebosim.org/tutorials?tut=ros_gzplugins>`__
- `Intel RealSense
  ROS包 <https://github.com/IntelRealSense/realsense-ros>`__
- `Livox ROS驱动 <https://github.com/Livox-SDK/livox_ros_driver2>`__
- `SDF格式规范 <http://sdformat.org/spec>`__
- `ROS TF教程 <http://wiki.ros.org/tf/Tutorials>`__