ROS2+Gazebo+ArduSub构建高保真水下机器人仿真平台

水下机器人开发，最让人头疼的是什么？不是算法本身，而是“测试”。你精心设计了一套自主导航算法，难道要直接扔进海里验证？成本高、风险大、环境不可控，一次失败可能就是数万元的损失。这就是为什么仿真（Simulation）在机器人领域，尤其是水下机器人领域，变得如此关键。

但仿真本身也是个技术活。很多开发者会陷入一个误区：以为只要在Gazebo里放个模型，写个ROS节点控制它动起来，就算仿真成功了。结果往往是模型在水里“飘”得不像话，控制响应诡异，传感器数据失真，仿真和现实完全脱节。问题的核心在于，你搭建的只是一个“视觉外壳”，缺少了真实机器人的“灵魂”——即底层的动力、浮力、推进器模型以及成熟、稳定的飞控逻辑。

这篇文章要解决的，正是这个痛点。我们将介绍一种“三剑合璧”的方案：ROS2 + Gazebo + ArduSub。这个组合的核心价值在于：用ROS2做上层应用和通信框架，用Gazebo提供高保真的物理与传感器仿真环境，而用ArduSub——这个经过无数真实水下机器人验证的开源飞控固件——来充当仿真机器人的“大脑”和“小脑”。它负责处理最复杂的底层动力学、推进器混控、姿态稳定和传感器融合。你的算法只需要通过ROS2与ArduSub通信，发出高级指令（如“前进1米”、“下潜到5米深度”），剩下的脏活累活都交给这个久经考验的飞控。

读完本文，你将能搭建一个接近真实水下机器人动力学特性的仿真环境，并在此之上开发、测试你的自主控制算法（如定深、定高、路径跟踪）。这不仅能将你的开发成本降低几个数量级，更能让你在将代码部署到实体机器人之前，就拥有充足的信心。

1. 为什么是 ROS2 + Gazebo + ArduSub？

在深入技术细节前，我们先厘清这三个组件各自扮演的角色，以及它们组合起来的化学反应。

ROS2 (Robot Operating System 2)：机器人领域的“神经系统”和“中间件”。它负责节点（Node）间的通信（Topic, Service, Action），提供工具链（如Rviz2用于可视化，ros2 bag用于数据记录），并管理整个软件系统的生命周期。在仿真中，ROS2是你所有自主控制算法、决策逻辑的载体，它通过标准接口与仿真世界交互。

Gazebo：机器人领域的“虚拟沙盒”和“物理引擎”。它提供高保真的3D渲染、刚体动力学模拟、传感器模型（如IMU、深度计、摄像头、声呐）以及环境模拟（如水下光照、浮力、流体阻尼）。Gazebo负责创造一个尽可能真实的物理世界，让你的机器人模型在其中接受“考验”。

ArduSub：专为水下机器人（ROV, AUV）设计的开源自动驾驶仪固件。它源自著名的ArduPilot项目，集成了姿态估计（AHRS）、深度控制、位置保持、自动任务执行等核心功能。在真实世界中，它运行在Pixhawk等硬件上；在仿真中，它可以作为一个“软件在环”（SITL）实例运行。

传统做法的局限：通常，开发者会在Gazebo中为机器人模型编写插件（Plugin），直接接收ROS2的控制指令（如每个推进器的推力），并计算动力学响应。这要求开发者对水下机器人的流体力学、推进器布局、混控算法有极深的了解，且调试复杂，极易出错。

新方案的优势：

1. 真实性：ArduSub的动力学和控制器模型经过大量真实数据调校和验证，其仿真行为与真实机器人高度一致。
2. 分工明确：Gazebo只负责“渲染物理世界”和“模拟传感器数据”；ArduSub负责“解读传感器数据”并“计算控制输出”；ROS2负责“高级任务编排”和“通信桥梁”。各司其职，复杂度被隔离。
3. 开发效率：你可以直接使用MAVLink协议（ArduSub使用的通信协议）或ROS2-MAVLink桥接工具，向ArduSub发送诸如SET_POSITION_TARGET_LOCAL_NED这样的高级指令，而无需关心每个推进器该如何转动。
4. 技能复用：为仿真环境开发的ROS2节点和算法，可以几乎不做修改地部署到运行ArduSub的真实硬件上，实现了从仿真到实物的平滑过渡。

简单来说，这套组合拳让你能用开发无人机或地面机器人的思路和效率，来开发水下机器人，大幅降低了领域专业门槛。

2. 核心概念与工作流程

理解整个仿真系统如何联动是关键。下图展示了数据流与控制流：

[你的ROS2控制节点] --(ROS2 Topic)--> [MAVROS/ROS2-MAVLink桥] --(MAVLink/UDP)--> [ArduSub SITL] | V [Gazebo仿真环境] <--(传感器数据/物理状态)--> [ArduSub SITL] --(执行器输出)--> [Gazebo仿真环境] | V [Gazebo仿真环境] --(模拟的传感器数据)--> [ArduSub SITL]

关键组件解释：

• ArduSub SITL (Software In The Loop)：这是ArduSub固件在普通电脑上运行的版本。它以为自己连接着真实的传感器（IMU、压力计）和执行器（电调、推进器），但实际上这些数据来自Gazebo，输出也发送给Gazebo。
• MAVLink协议：一种轻量级的消息传递协议，专为无人机/无人车系统设计。ArduSub使用MAVLink与地面站或其他控制器通信。
• 桥接工具 (MAVROS / ros2-mavros)：这是一个ROS/ROS2功能包，它在ROS2的ROS话题（Topic）和MAVLink消息之间进行转换。你的ROS2节点发布一个ROS标准几何消息（如geometry_msgs/msg/PoseStamped），桥接工具会将其转换为MAVLink的SET_POSITION_TARGET_LOCAL_NED命令，通过UDP发送给ArduSub SITL。
• Gazebo插件 (libArduPilotPlugin)：这是一个Gazebo世界插件，它充当了Gazebo和ArduSub SITL之间的“翻译官”。它从Gazebo获取机器人的姿态、速度等状态，封装成MAVLink的HIL_STATE_QUATERNION等消息发送给SITL；同时，它接收SITL发来的HIL_ACTUATOR_CONTROLS（执行器控制量），并将其施加到Gazebo模型对应的关节（推进器）上。

工作流程：

1. Gazebo启动，加载水下机器人模型和世界。
2. ArduSub SITL启动，等待连接。
3. Gazebo的ArduPilot插件通过UDP连接到SITL，开始交换数据。
4. ROS2-MAVLink桥接启动，连接到同一个SITL实例。
5. 你的ROS2控制节点通过ROS2话题发布目标位置或速度。
6. 桥接工具将ROS2消息转为MAVLink命令，发给SITL。
7. SITL根据当前状态（来自Gazebo）和目标命令，计算出发动机控制量，发回给Gazebo插件。
8. Gazebo插件驱动模型关节运动，模型状态更新。
9. 更新的状态（作为传感器数据）再次被发送给SITL，形成闭环。

3. 环境准备与安装

我们将基于Ubuntu 22.04和ROS2 Humble进行环境搭建。这是目前（2024年）较为稳定和流行的组合。

3.1 安装ROS2 Humble

如果系统已安装ROS2 Humble，可跳过此步。

# 设置locale sudo apt update && sudo apt install locales sudo locale-gen en_US en_US.UTF-8 sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 export LANG=en_US.UTF-8 # 添加ROS2仓库 sudo apt install software-properties-common sudo add-apt-repository universe sudo apt update && sudo apt install curl -y sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null # 安装ROS2桌面版 sudo apt update sudo apt install ros-humble-desktop python3-colcon-common-extensions -y # 配置环境变量 source /opt/ros/humble/setup.bash echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc

3.2 安装Gazebo Harmonic

Gazebo Harmonic是Gazebo的最新系列，与ROS2 Humble集成更好。我们通过ROS来安装。

# 安装Gazebo Harmonic及相关ROS包 sudo apt install ros-humble-gazebo-ros-pkgs ros-humble-gazebo-ros -y # 验证安装 gazebo --version # 应输出类似：gazebo version 11.x.x

3.3 安装ArduSub SITL及相关工具

我们需要安装ArduPilot的仿真环境，这包括ArduSub固件源码和必要的Python依赖。

# 1. 安装必要的依赖 sudo apt-get install git python3-pip python3-dev python3-opencv python3-wxgtk4.0 python3-matplotlib python3-lxml python3-pygame python3-yaml python3-pexpect -y sudo pip3 install future pymavlink MAVProxy # 2. 克隆ArduPilot源码（包含ArduSub） cd ~ git clone https://github.com/ArduPilot/ardupilot.git cd ardupilot git submodule update --init --recursive # 3. 安装ArduPilot专属依赖 Tools/environment_install/install-prereqs-ubuntu.sh -y # 执行后需要重新登录或source ~/.profile使环境变量生效 # 4. 切换到稳定分支（例如Copter-4.4，Sub的版本通常跟随Copter） git checkout Copter-4.4

3.4 安装ROS2与MAVLink桥接 (MAVROS)

我们需要ros-humble-mavros和ros-humble-mavros-msgs包。

sudo apt install ros-humble-mavros ros-humble-mavros-msgs ros-humble-geographic-msgs -y # 安装地理库依赖（MAVROS需要） sudo apt install geographiclib-tools -y sudo geographiclib-get-geoids egm96-5

4. 获取并配置水下机器人仿真模型

一个高质量的仿真模型是成功的一半。我们可以使用社区维护的BlueROV2模型，这是一个非常流行的开源水下机器人。

# 创建一个工作空间 mkdir -p ~/subsim_ws/src cd ~/subsim_ws/src # 克隆包含BlueROV2 Gazebo模型和世界的仓库 git clone https://github.com/osrf/bluerov2_ignition.git # 注意：这个仓库可能使用Ignition Gazebo（Gazebo的新名称），但模型通常与Gazebo Classic兼容。 # 我们也可以使用其他来源，例如UUV Simulator或自定义模型。 # 更通用的方法是，我们可以直接配置一个简单的模型。 # 在 ~/subsim_ws/src 下创建一个功能包 ros2 pkg create --build-type ament_cmake bluerov_gazebo cd bluerov_gazebo mkdir -p models launch worlds

我们需要创建几个关键文件：

1. 机器人模型文件 (models/bluerov2/model.sdf): 这是一个简化的BlueROV2模型，定义了外观、碰撞属性、关节（推进器）和传感器插件。由于文件较长，这里给出关键部分框架，完整文件需从可靠来源获取或根据Gazebo文档编写。

<!-- 文件：~/subsim_ws/src/bluerov_gazebo/models/bluerov2/model.sdf --> <?xml version="1.0" ?> <sdf version="1.6"> <model name="bluerov2"> <pose>0 0 0 0 0 0</pose> <static>false</static> <link name="base_link"> <pose>0 0 0 0 0 0</pose> <inertial> <mass>10.0</mass> <!-- 质量，单位kg --> <inertia> <ixx>0.5</ixx> <ixy>0</ixy> <ixz>0</ixz> <iyy>0.5</iyy> <iyz>0</iyz> <izz>0.5</izz> </inertia> </inertial> <collision name="collision"> <geometry><box><size>0.5 0.3 0.2</size></box></geometry> </collision> <visual name="visual"> <geometry><mesh><uri>file://meshes/bluerov2_body.dae</uri></mesh></geometry> </visual> <!-- 传感器定义：IMU --> <sensor name="imu_sensor" type="imu"> <always_on>true</always_on> <update_rate>100</update_rate> <imu> <topic>/bluerov2/imu</topic> <angular_velocity> <x> <noise type="gaussian"> <mean>0.0</mean> <stddev>0.0002</stddev> </noise> </x> <!-- ... y, z 轴类似 --> </angular_velocity> <linear_acceleration> <x> <noise type="gaussian"> <mean>0.0</mean> <stddev>0.017</stddev> </noise> </x> <!-- ... y, z 轴类似 --> </linear_acceleration> </imu> </sensor> </link> <!-- 定义6个推进器关节 (基于BlueROV2布局) --> <joint name="thruster_front_left" type="revolute"> <parent>base_link</parent> <child>thruster_front_left_link</child> <axis> <xyz>0 0 1</xyz> <limit> <lower>-1e16</lower> <upper>1e16</upper> </limit> <dynamics> <damping>0.01</damping> </dynamics> </axis> </joint> <!-- 为每个关节定义link和Gazebo插件，用于接收ArduSub控制信号 --> <plugin name="thruster_front_left" filename="libArduPilotPlugin.so"> <robotNamespace></robotNamespace> <jointName>thruster_front_left</jointName> <motorNumber>0</motorNumber> <!-- 对应ArduSub的电机编号 --> <direction>1</direction> </plugin> <!-- ... 其他5个推进器关节和插件定义类似，motorNumber从0到5 --> <!-- Gazebo ArduPilot插件，负责与SITL通信 --> <plugin name="ardupilot" filename="libArduPilotPlugin.so"> <robotNamespace></robotNamespace> <fdm_addr>127.0.0.1</fdm_addr> <!-- SITL的IP --> <fdm_port_in>9002</fdm_port_in> <!-- SITL接收HIL数据的端口 --> <fdm_port_out>9003</fdm_port_out> <!-- SITL发送执行器数据的端口 --> </plugin> </model> </sdf>

2. 仿真世界文件 (worlds/underwater.world): 定义一个水下环境。

<!-- 文件：~/subsim_ws/src/bluerov_gazebo/worlds/underwater.world --> <?xml version="1.0" ?> <sdf version="1.6"> <world name="underwater"> <!-- 物理引擎参数，对水下仿真很重要 --> <physics name="underwater_physics" type="ode"> <max_step_size>0.001</max_step_size> <real_time_factor>1.0</real_time_factor> <real_time_update_rate>1000</real_time_update_rate> <ode> <solver> <type>quick</type> <iters>50</iters> <sor>1.3</sor> </solver> <constraints> <cfm>0.0</cfm> <erp>0.2</erp> <contact_max_correcting_vel>100</contact_max_correcting_vel> <contact_surface_layer>0.001</contact_surface_layer> </constraints> </ode> </physics> <!-- 水下环境：重力、浮力、流体阻尼 --> <include> <uri>model://sun</uri> </include> <include> <uri>model://ground_plane</uri> </include> <!-- 添加一个海底地形 --> <model name="seafloor"> <static>true</static> <link name="link"> <collision name="collision"> <geometry> <box> <size>50 50 0.1</size> </box> </geometry> </collision> <visual name="visual"> <geometry> <box> <size>50 50 0.1</size> </box> </geometry> <material> <script> <uri>file://media/materials/scripts/gazebo.material</uri> <name>Gazebo/Sandy</name> </script> </material> </visual> </link> </model> <!-- 水的全局属性（通过Gazebo的浮力插件实现，这里简化，实际需插件） --> <plugin name="buoyancy" filename="libBuoyancyPlugin.so"> <fluid_density>1025.0</fluid_density> <!-- 海水密度 kg/m^3 --> </plugin> <!-- 在这里包含我们的机器人模型 --> <include> <uri>model://bluerov2</uri> <pose>0 0 -5 0 0 0</pose> <!-- 初始位置在水下5米 --> </include> </world> </sdf>

3. Launch文件 (launch/start_simulation.launch.py): 用于一键启动所有组件。

# 文件：~/subsim_ws/src/bluerov_gazebo/launch/start_simulation.launch.py import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ExecuteProcess, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node def generate_launch_description(): # 获取功能包路径 pkg_bluerov_gazebo = get_package_share_directory('bluerov_gazebo') # 1. 启动Gazebo服务器和客户端，加载水下世界 gazebo_world_path = os.path.join(pkg_bluerov_gazebo, 'worlds', 'underwater.world') gazebo_cmd = ExecuteProcess( cmd=['gazebo', '--verbose', gazebo_world_path, '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'], output='screen' ) # 2. 在后台启动ArduSub SITL # 注意：需要先切换到ardupilot目录并配置好环境 sitl_cmd = ExecuteProcess( cmd=['bash', '-c', 'cd ~/ardupilot && ./Tools/autotest/sim_vehicle.py -v ArduSub -f vehicle --console --map --out=udp:127.0.0.1:14550'], output='screen', shell=True ) # 3. 启动MAVROS节点，连接SITL mavros_node = Node( package='mavros', executable='mavros_node', name='mavros', output='screen', parameters=[{ 'fcu_url': 'udp://:14550@127.0.0.1:14555', 'gcs_url': '', 'target_system_id': 1, 'target_component_id': 1, 'fcu_protocol': 'v2.0', }] ) # 4. (可选) 启动一个简单的ROS2控制节点示例 example_controller_node = Node( package='bluerov_gazebo', # 假设我们后续会创建这个节点 executable='simple_depth_hold', name='depth_controller', output='screen' ) return LaunchDescription([ gazebo_cmd, # 可以稍等Gazebo启动再启动SITL，这里简化处理 sitl_cmd, mavros_node, # example_controller_node, ])

5. 编写一个简单的自主控制节点

现在，我们编写一个最简单的ROS2节点，它通过MAVROS让机器人保持在一定深度。

首先，在功能包中创建节点源文件。

cd ~/subsim_ws/src/bluerov_gazebo/src touch simple_depth_hold.cpp

// 文件：~/subsim_ws/src/bluerov_gazebo/src/simple_depth_hold.cpp #include <rclcpp/rclcpp.hpp> #include <mavros_msgs/msg/altitude.hpp> #include <mavros_msgs/msg/position_target.hpp> #include <geometry_msgs/msg/pose_stamped.hpp> #include <std_msgs/msg/float64.hpp> using namespace std::chrono_literals; class DepthHoldNode : public rclcpp::Node { public: DepthHoldNode() : Node("depth_hold_node") { // 订阅当前高度/深度信息 (来自MAVROS的/altitude话题) altitude_sub_ = this->create_subscription<mavros_msgs::msg::Altitude>( "/mavros/altitude", 10, std::bind(&DepthHoldNode::altitudeCallback, this, std::placeholders::_1)); // 发布位置设定点 (发送给MAVROS，转换为MAVLink命令) setpoint_pub_ = this->create_publisher<mavros_msgs::msg::PositionTarget>( "/mavros/setpoint_raw/local", 10); // 定时器，用于持续发布控制指令 timer_ = this->create_wall_timer( 100ms, std::bind(&DepthHoldNode::timerCallback, this)); // 目标深度 (相对于水面，负值表示水下)，例如-2.0米 target_depth_ = -2.0; current_altitude_ = 0.0; RCLCPP_INFO(this->get_logger(), "Depth Hold Node Started. Target depth: %.2f m", target_depth_); } private: void altitudeCallback(const mavros_msgs::msg::Altitude::SharedPtr msg) { // MAVROS的Altitude消息中，`local`字段通常表示相对于起飞点的高度。 // 对于水下，我们更关心相对于水面的深度。这里假设水面高度为0。 // 在SITL中，通常将水面作为Z=0平面，水下为负。 // `terrain` 或 `bottom` 字段可能表示到海底的距离，这里使用`local`。 current_altitude_ = msg->local; // RCLCPP_DEBUG(this->get_logger(), "Current altitude: %.2f", current_altitude_); } void timerCallback() { // 简单的P控制器：深度误差 = 目标深度 - 当前高度 // 注意：current_altitude_在水下为负值，target_depth_也为负值。 // 误差为正表示当前在目标上方，需要下潜。 double depth_error = target_depth_ - current_altitude_; double climb_rate_cmd = depth_error * 0.5; // P增益，需调试 // 限制爬升率命令 climb_rate_cmd = std::max(-0.5, std::min(0.5, climb_rate_cmd)); // 构造PositionTarget消息 auto setpoint_msg = mavros_msgs::msg::PositionTarget(); setpoint_msg.header.stamp = this->now(); setpoint_msg.coordinate_frame = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED; setpoint_msg.type_mask = mavros_msgs::msg::PositionTarget::IGNORE_PX | mavros_msgs::msg::PositionTarget::IGNORE_PY | mavros_msgs::msg::PositionTarget::IGNORE_PZ | mavros_msgs::msg::PositionTarget::IGNORE_AFX | mavros_msgs::msg::PositionTarget::IGNORE_AFY | mavros_msgs::msg::PositionTarget::IGNORE_AFZ | mavros_msgs::msg::PositionTarget::IGNORE_YAW | mavros_msgs::msg::PositionTarget::IGNORE_YAW_RATE; // 我们只控制垂直方向的速度 setpoint_msg.velocity.z = climb_rate_cmd; // FRAME_LOCAL_NED下，Z轴向下为正。下潜需要正速度。 setpoint_pub_->publish(setpoint_msg); // RCLCPP_DEBUG(this->get_logger(), "Publishing climb rate: %.2f m/s", climb_rate_cmd); } rclcpp::Subscription<mavros_msgs::msg::Altitude>::SharedPtr altitude_sub_; rclcpp::Publisher<mavros_msgs::msg::PositionTarget>::SharedPtr setpoint_pub_; rclcpp::TimerBase::SharedPtr timer_; double target_depth_; double current_altitude_; }; int main(int argc, char **argv) { rclcpp::init(argc, argv); auto node = std::make_shared<DepthHoldNode>(); rclcpp::spin(node); rclcpp::shutdown(); return 0; }

接下来，修改CMakeLists.txt和package.xml以编译此节点。

package.xml添加依赖：

<!-- 在 ~/subsim_ws/src/bluerov_gazebo/package.xml 的 <buildtool_depend> 后添加 --> <depend>rclcpp</depend> <depend>geometry_msgs</depend> <depend>mavros_msgs</depend> <depend>std_msgs</depend>

CMakeLists.txt添加可执行文件：

# 在 ~/subsim_ws/src/bluerov_gazebo/CMakeLists.txt 的 find_package 部分添加 find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(mavros_msgs REQUIRED) find_package(std_msgs REQUIRED) # 添加可执行文件和目标依赖 add_executable(simple_depth_hold src/simple_depth_hold.cpp) ament_target_dependencies(simple_depth_hold rclcpp geometry_msgs mavros_msgs std_msgs ) # 安装目标 install(TARGETS simple_depth_hold DESTINATION lib/${PROJECT_NAME} )

最后，编译工作空间：

cd ~/subsim_ws colcon build --packages-select bluerov_gazebo source install/setup.bash

6. 启动仿真与测试

现在，让我们启动整个系统，并观察机器人是否能稳定在目标深度。

步骤1：启动ArduSub SITL（在一个终端中）

cd ~/ardupilot # 启动ArduSub SITL，连接到Gazebo的默认端口 ./Tools/autotest/sim_vehicle.py -v ArduSub -f vehicle --console --map --out=udp:127.0.0.1:14550

等待直到看到APM: ArduSub Ready之类的提示，并且控制台显示PreArm: RC not calibrated等是正常的。

步骤2：启动Gazebo仿真世界（在另一个终端中）

cd ~/subsim_ws source install/setup.bash # 使用我们写的launch文件，或者直接启动gazebo并加载模型 ros2 launch bluerov_gazebo start_simulation.launch.py # 或者分开启动： # 终端2.1: 启动Gazebo服务器 # gazebo --verbose worlds/underwater.world # 终端2.2: 启动MAVROS # ros2 run mavros mavros_node __params:=path_to_your_mavros_config.yaml

如果一切正常，Gazebo窗口会打开，显示水下环境和BlueROV2模型。

步骤3：解锁并切换飞行模式（在SITL终端中）在SITL的控制台（sim_vehicle.py启动的那个终端）中，你需要给ArduSub发送指令，让它从初始化状态进入可控制状态。

# 在SITL控制台中输入： mode guided # 等待显示“Mode GUIDED” arm throttle # 等待显示“ARMING MOTORS”和“Armed”

现在，飞控已经准备就绪，等待接收来自MAVROS（也就是你的ROS2节点）的控制指令。

步骤4：运行深度保持节点（在第三个终端中）

cd ~/subsim_ws source install/setup.bash ros2 run bluerov_gazebo simple_depth_hold

预期现象：

1. 在Gazebo中，你会看到机器人模型。
2. 由于初始位置在水下5米（我们在world文件中设置的<pose>0 0 -5 0 0 0</pose>），而目标深度是-2米，机器人会上浮。
3. 观察Gazebo左侧的“World”选项卡，找到你的机器人模型（bluerov2），展开其base_link，查看pose中的z值。它应该从-5逐渐向-2变化，并最终在-2米附近震荡（由于是简单的P控制）。
4. 你可以在SITL控制台使用status命令查看详细状态，或在ROS2中使用ros2 topic echo /mavros/altitude查看深度信息。

验证控制是否生效：

• 在ROS2中，监听控制指令话题：ros2 topic echo /mavros/setpoint_raw/local，应该能看到周期性的消息，其中velocity.z字段在变化。
• 在Gazebo中，你可以尝试在运行时修改代码中的target_depth_（比如改为-4.0），重新编译运行节点，观察机器人是否开始下潜。

7. 常见问题与排查思路

搭建这套环境可能遇到各种问题，下表列出了常见现象及解决方法：

8. 最佳实践与进阶方向

当你成功运行基础仿真后，可以考虑以下优化和进阶步骤，让仿真更真实、开发更高效。

8.1 仿真环境优化

• 添加水流和海洋扰动：Gazebo可以通过插件模拟恒定流或随机扰动。这能测试控制器的鲁棒性。
• 使用高保真传感器模型：为摄像头添加水下光学失真、为声呐添加噪声和射束模式、为DVL（多普勒计程仪）添加底跟踪失效模型。
• 引入真实地形与障碍物：导入真实海域的DEM（数字高程模型）数据，或创建管道、沉船等复杂场景，用于测试避障和路径规划算法。

8.2 控制器开发进阶

• 实现完整的自主任务：结合ROS2的导航栈（Nav2）或行为树（Behavior Tree），实现“从A点到B点”、“区域扫描”、“目标跟踪”等复杂任务。
• 集成外部控制器：你的ROS2节点可以订阅摄像头或声呐的点云/图像数据，进行目标检测，然后输出目标位置给MAVROS，实现基于视觉的跟踪。
• 参数调试与优化：利用ROS2的参数服务器（Parameter Server）动态调整控制器的PID参数，无需重新编译。使用rqt_reconfigure工具进行图形化调参。

8.3 从仿真到实物的过渡

• 硬件在环（HITL）测试：将SITL替换为真实的Pixhawk硬件，Gazebo的输出通过USB/UART发送给飞控，飞控的真实输出被Gazebo读取。这是部署前最关键的一步。
• 统一配置管理：使用ROS2的launch文件和参数文件来管理仿真与实物部署的不同配置（如传感器话题名称、控制器增益、通信端口）。
• 日志与数据分析：在仿真中大量使用ros2 bag记录所有话题数据。这些数据可以用于离线分析、算法验证，甚至训练机器学习模型。

8.4 工程化建议

• 版本控制：将你的机器人模型、世界文件、启动文件和ROS2节点代码全部纳入Git管理。为ArduSub固件版本、Gazebo版本和ROS2版本建立明确的依赖关系。
• 持续集成：可以设置GitHub Actions或GitLab CI，在代码提交后自动启动仿真，运行一系列自动化测试（如“能否到达目标点”、“是否发生碰撞”），确保算法修改不会引入回归错误。
• 文档与协作：为你的仿真项目编写清晰的README，说明环境搭建步骤、启动顺序、常用测试用例。这对于团队协作至关重要。

通过将ROS2的灵活性与Gazebo的保真度、ArduSub的可靠性相结合，你构建的不仅仅是一个仿真环境，而是一个完整的水下机器人算法研发、测试和验证平台。这个平台能显著缩短开发周期，降低试错成本，并最终提高你部署到真实水下机器人上的代码质量。