ROS 2 与 Isaac Sim 联合仿真（二）传感器、控制闭环与典型算法栈

对于移动机器人，典型目标是让 Isaac Sim 发布 /scan、/odom、/tf、/clock 等信息，由 ROS2 Nav2 输出 /cmd_vel 驱动仿真机器人运动。
对于机械臂，目标是让 Isaac Sim 发布 /joint_states 和 TF，由 MoveIt 2 或 ros2_control 生成关节轨迹，再通过 Isaac Sim 的 Articulation Controller 执行。

联合仿真的终极目标是Sim-to-Real 零修改迁移：在仿真中验证通过的代码，无需修改即可直接部署到真实机器人上。这要求仿真系统不仅要"能通信"，更要在数据分布、接口规范、时序特性和动力学响应四个维度与真实硬件保持一致。

1. 传感器链路

NVIDIA 官方研究表明，当仿真传感器的数据分布与真实传感器的差异小于 5% 时，Sim-to-Real 迁移成功率可以达到 90% 以上。反之，如果传感器数据存在系统性偏差，即使算法在仿真中表现完美，在真实世界中也会完全失效。Isaac Sim 的 RTX 传感器系列正是为了解决这一问题而设计，它们模拟了真实传感器的物理成像过程，而不仅仅是渲染图像。

常见传感器数据包括：

RGB Camera: sensor_msgs/msg/Image
Depth Camera: sensor_msgs/msg/Image
Camera Info: sensor_msgs/msg/CameraInfo
LiDAR Scan: sensor_msgs/msg/LaserScan
Point Cloud: sensor_msgs/msg/PointCloud2
IMU: sensor_msgs/msg/Imu
Odometry: nav_msgs/msg/Odometry
TF: tf2_msgs/msg/TFMessage
Force-Torque: geometry_msgs/msg/WrenchStamped
Contact: sensor_msgs/msg/ContactState

传感器链路的关键不是"能发布"，而是发布的数据是否在 ROS 2 侧可解释、可同步、可用于算法闭环。一个合格的仿真传感器应该能够复现真实传感器的所有缺陷，包括噪声、畸变、延迟和数据丢失。

2. 相机数据发布

Isaac Sim 支持 RGB、Depth、PointCloud、语义分割、实例分割、2D/3D bounding box 等多种相机相关数据。在 ROS 2 侧，最常见的是：

/camera/image_raw
/camera/camera_info
/depth/image_raw
/depth/camera_info
/camera/points

2.1 RTX 相机与非 RTX 相机的区别

对于所有需要处理视觉数据的算法，强烈推荐使用 RTX 相机。虽然它会消耗更多的 GPU 资源，但带来的 Sim-to-Real 收益是巨大的。

2.2 相机内参与外参

Isaac Sim 会根据相机的物理参数自动生成正确的 camera_info 消息。关键参数包括：

• focalLength：实际焦距（mm）fx,fy是像素焦距
• horizontalAperture：水平传感器尺寸（mm）
• verticalAperture：垂直传感器尺寸（mm）
• resolution：图像分辨率（像素）

相机内参计算公式为：

fx = focalLength * resolution_x / horizontalAperture
fy = focalLength * resolution_y / verticalAperture
cx = resolution_x / 2
cy = resolution_y / 2

相机外参由相机在 USD 场景中的位置和方向决定，应该与真实机器人上的传感器安装位置完全一致。外参错误会导致 SLAM 漂移、目标定位错误和抓取失败。

2.3 时间同步

RGB 和 Depth 相机的时间戳必须完全一致，否则会导致深度与颜色不匹配。在 OmniGraph 中，应该使用同一个 On Playback Tick 节点同时触发 RGB 和 Depth 的发布。在 C++ 中，应该在同一个仿真步中获取并发布两种数据。

C++ 代码示例：发布 RGB 相机数据

#include <omni/isaac/sensors/Camera.h>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/camera_info.hpp>

// 创建相机
auto camera = world.scene().add<sensors::Camera>(
"/World/Jetbot/camera",
"camera",
Eigen::Vector3d(0.1, 0.0, 0.1),
Eigen::Quaterniond::Identity()
);
camera->set_resolution(640, 480);
camera->set_focal_length(3.0); // 3mm 焦距

// 创建发布器
auto image_pub = ros2_bridge.create_publisher<sensor_msgs::msg::Image>(
"/camera/image_raw",
rclcpp::QoS(10).best_effort()
);
auto camera_info_pub = ros2_bridge.create_publisher<sensor_msgs::msg::CameraInfo>(
"/camera/camera_info",
rclcpp::QoS(10).transient_local()
);

// 在仿真循环中发布
while (simulation_app.is_running()) {
world.step(render::RenderMode::kFull);

if (world.is_playing() && camera->is_available()) {
double current_time = world.current_time();

// 获取 RGB 图像
auto rgb_image = camera->get_rgba_image();

// 发布 Image 消息
sensor_msgs::msg::Image image_msg;
image_msg.header.stamp = rclcpp::Time(current_time);
image_msg.header.frame_id = "camera_link";
image_msg.height = rgb_image.height();
image_msg.width = rgb_image.width();
image_msg.encoding = "rgba8";
image_msg.step = rgb_image.width() * 4;
image_msg.data = std::vector<uint8_t>(
rgb_image.data(),
rgb_image.data() + rgb_image.size()
);
image_pub->publish(image_msg);

// 发布 CameraInfo 消息
sensor_msgs::msg::CameraInfo camera_info_msg;
camera_info_msg.header = image_msg.header;
camera_info_msg.width = rgb_image.width();
camera_info_msg.height = rgb_image.height();
camera_info_msg.k = {
camera->get_focal_length_x(), 0.0, camera->get_principal_point_x(),
0.0, camera->get_focal_length_y(), camera->get_principal_point_y(),
0.0, 0.0, 1.0
};
camera_info_msg.p = {
camera->get_focal_length_x(), 0.0, camera->get_principal_point_x(), 0.0,
0.0, camera->get_focal_length_y(), camera->get_principal_point_y(), 0.0,
0.0, 0.0, 1.0, 0.0
};
camera_info_pub->publish(camera_info_msg);
}
}

2.4 工程检查与优化

工程检查命令：

ros2 topic list | grep camera
ros2 topic hz /camera/image_raw
ros2 topic echo /camera/camera_info
ros2 run rqt_image_view rqt_image_view

相机数据要特别关注以下内容：

推荐频率：

RGB: 15~30 Hz
Depth: 10~30 Hz
语义分割: 按任务需求降低频率（通常 1~5 Hz）
点云: 5~20 Hz

不要盲目追求高分辨率和高频率。一个 1920×1080 分辨率的 RGB 相机以 30Hz 发布时，会产生大约 200MB/s 的数据流量，这会迅速耗尽 DDS 带宽并导致延迟堆积。对于大多数导航和避障任务，640×480 分辨率已经足够。

3. LiDAR 与点云发布

Isaac Sim 的 RTX LiDAR 可模拟旋转式和固态 LiDAR，并可发布：

/scan sensor_msgs/msg/LaserScan
/point_cloud sensor_msgs/msg/PointCloud2

3.1 RTX LiDAR 工作原理

RTX LiDAR 模拟了真实激光雷达的光子发射和接收过程，能够准确模拟：

• 多径效应
• 物体反射率
• 距离噪声
• 角度分辨率
• 最大探测距离
• 最小探测距离

Isaac Sim 内置了多种主流 LiDAR 模型的预设，包括：

• Velodyne VLP-16、VLP-32C、HDL-64E
• Ouster OS1-64、OS2-128
• Hesai AT128、QT128
• RoboSense RS-LiDAR-16、RS-M1

使用预设可以确保仿真 LiDAR 的参数与真实硬件完全一致，大幅提高 Sim-to-Real 效果。

3.2 LaserScan 与 PointCloud2 的选择

• LaserScan：2D 激光扫描数据，只包含距离和角度信息。数据量小，处理速度快，是 Nav2 导航的标准输入。
• PointCloud2：3D 点云数据，包含 X、Y、Z 坐标、强度、反射率等信息。数据量大，但提供了更丰富的环境信息，适用于 3D 避障、三维建图和语义感知。

对于纯 2D 导航任务，只需要发布 /scan 话题即可。对于需要 3D 感知的任务，应该发布 /point_cloud 话题，并使用 pointcloud_to_laserscan 包将点云转换为 LaserScan 供 Nav2 使用。

3.3 点云数据优化

点云数据是所有传感器数据中体量最大的，必须进行优化才能保证系统实时性：

C++ 代码示例：发布 LiDAR 数据

#include <omni/isaac/sensors/RtxLidar.h>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

// 创建 RTX LiDAR
auto lidar = world.scene().add<sensors::RtxLidar>(
"/World/Jetbot/lidar",
"lidar",
Eigen::Vector3d(0.0, 0.0, 0.15),
Eigen::Quaterniond::Identity()
);
lidar->load_preset("Velodyne_VLP_16");
lidar->set_rotation_frequency(10.0); // 10Hz 旋转频率

// 创建发布器
auto scan_pub = ros2_bridge.create_publisher<sensor_msgs::msg::LaserScan>(
"/scan",
rclcpp::QoS(10).best_effort()
);

// 在仿真循环中发布
while (simulation_app.is_running()) {
world.step(render::RenderMode::kFull);

if (world.is_playing() && lidar->is_available()) {
double current_time = world.current_time();

// 获取 LaserScan 数据
auto scan_data = lidar->get_laser_scan();

// 发布 LaserScan 消息
sensor_msgs::msg::LaserScan scan_msg;
scan_msg.header.stamp = rclcpp::Time(current_time);
scan_msg.header.frame_id = "lidar_link";
scan_msg.angle_min = scan_data.angle_min();
scan_msg.angle_max = scan_data.angle_max();
scan_msg.angle_increment = scan_data.angle_increment();
scan_msg.time_increment = scan_data.time_increment();
scan_msg.scan_time = scan_data.scan_time();
scan_msg.range_min = scan_data.range_min();
scan_msg.range_max = scan_data.range_max();
scan_msg.ranges = scan_data.ranges();
scan_msg.intensities = scan_data.intensities();
scan_pub->publish(scan_msg);
}
}

3.4 工程检查

检查命令：

ros2 topic hz /scan
ros2 topic echo /scan
ros2 topic hz /point_cloud
rviz2

LiDAR 数据需要重点关注：

4. IMU、接触与力传感器

IMU 通常发布为：

/imu sensor_msgs/msg/Imu

4.1 IMU 噪声模型与校准

Isaac Sim 支持配置 IMU 的完整噪声模型，包括：

• 高斯白噪声（angular_velocity_noise、linear_acceleration_noise）
• 随机游走噪声（angular_velocity_bias、linear_acceleration_bias）
• 温度漂移
• 刻度因子误差

这些参数应该与真实 IMU 的 datasheet 完全一致。例如，MPU6050 的典型参数为：

• 角速度噪声密度：0.0038 rad/s/√Hz
• 加速度噪声密度：0.00025 g/√Hz
• 角速度随机游走：0.015 rad/s/√h
• 加速度随机游走：0.001 g/√h

正确配置噪声模型是保证 robot_localization、EKF 和 VIO 等算法在仿真中正常工作的关键。错误的噪声参数会导致滤波器发散或状态估计精度过低。

4.2 重力加速度处理

Isaac Sim 的 IMU 默认会包含重力加速度。在使用 robot_localization 等滤波器时，需要确保滤波器正确处理重力。通常，滤波器会假设 IMU 的线性加速度包含重力，并在内部进行补偿。

如果需要输出不含重力的线性加速度，可以在 IMU 传感器的属性中勾选 Remove Gravity 选项。但不推荐这样做，因为真实 IMU 都会输出包含重力的加速度。

4.3 力扭矩与接触传感器

力扭矩传感器和接触传感器常用于机械臂抓取、碰撞检测、强化学习和接触丰富任务。此类数据一般对仿真步长、物理参数、接触模型和控制频率更敏感。

• 力扭矩传感器：可以测量关节或末端执行器上的力和力矩。测量频率通常为 100-1000Hz。
• 接触传感器：可以检测物体之间的接触，并输出接触力、接触位置和接触法线。

C++ 代码示例：发布 IMU 数据

#include <omni/isaac/sensors/Imu.h>
#include <sensor_msgs/msg/imu.hpp>

// 创建 IMU 传感器
auto imu = world.scene().add<sensors::Imu>(
"/World/Jetbot/imu",
"imu",
Eigen::Vector3d(0.0, 0.0, 0.1),
Eigen::Quaterniond::Identity()
);
// 配置 IMU 噪声参数
imu->set_angular_velocity_noise(Eigen::Vector3d(0.0038, 0.0038, 0.0038));
imu->set_linear_acceleration_noise(Eigen::Vector3d(0.0025, 0.0025, 0.0025));

// 创建发布器
auto imu_pub = ros2_bridge.create_publisher<sensor_msgs::msg::Imu>(
"/imu",
rclcpp::QoS(10).best_effort()
);

// 在仿真循环中发布
while (simulation_app.is_running()) {
world.step(render::RenderMode::kFull);

if (world.is_playing() && imu->is_available()) {
double current_time = world.current_time();

// 获取 IMU 数据
auto imu_data = imu->get_imu_data();

// 发布 Imu 消息
sensor_msgs::msg::Imu imu_msg;
imu_msg.header.stamp = rclcpp::Time(current_time);
imu_msg.header.frame_id = "imu_link";

imu_msg.orientation.x = imu_data.orientation().x();
imu_msg.orientation.y = imu_data.orientation().y();
imu_msg.orientation.z = imu_data.orientation().z();
imu_msg.orientation.w = imu_data.orientation().w();

imu_msg.angular_velocity.x = imu_data.angular_velocity().x();
imu_msg.angular_velocity.y = imu_data.angular_velocity().y();
imu_msg.angular_velocity.z = imu_data.angular_velocity().z();

imu_msg.linear_acceleration.x = imu_data.linear_acceleration().x();
imu_msg.linear_acceleration.y = imu_data.linear_acceleration().y();
imu_msg.linear_acceleration.z = imu_data.linear_acceleration().z();

// 设置协方差矩阵
imu_msg.orientation_covariance = {0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01};
imu_msg.angular_velocity_covariance = {0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001};
imu_msg.linear_acceleration_covariance = {0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001};

imu_pub->publish(imu_msg);
}
}

4.4 工程检查

重点检查：

如果使用 robot_localization、EKF、VIO 或惯性导航算法，IMU 的时间戳和坐标方向尤其关键。错误的 IMU 坐标方向会导致状态估计在几秒钟内发散。

5. 发布频率与数据节流

Isaac Sim 的 OmniGraph 通常随仿真帧 tick。也就是说，默认情况下，很多 ROS 2 发布节点可能会以仿真帧率（通常 60Hz）发布数据。对于 /clock 和低带宽状态数据，这通常不是问题；但对于图像和点云，高频发布会迅速造成性能瓶颈。

5.1 推荐发布频率

根据 NVIDIA 官方的性能测试和工业实践，推荐的发布频率如下：

5.2 数据节流实现方法

C++ 数据节流示例：

double last_publish_time = 0.0;
const double publish_interval = 0.1; // 10Hz

while (simulation_app.is_running()) {
world.step(render::RenderMode::kFull);

if (world.is_playing()) {
double current_time = world.current_time();

if (current_time – last_publish_time >= publish_interval) {
// 发布数据
publish_camera_data();
publish_lidar_data();

last_publish_time = current_time;
}
}
}

5.3 性能优化策略

根据 NVIDIA 官方的测试数据，在 RTX 4090 显卡上，一个包含 1 个 640×480 RGB 相机（30Hz）、1 个 VLP-16 LiDAR（10Hz）和 1 个 IMU（100Hz）的移动机器人仿真，可以稳定运行在 60fps 以上。

6. QoS：传感器通信的关键配置

ROS 2 基于 DDS，QoS（Quality of Service）配置直接决定消息是否能被正确传递。常见 QoS 策略包括：

Reliability: Reliable / BestEffort
Durability: Volatile / TransientLocal
History: KeepLast / KeepAll
Depth: queue depth
Deadline
Lifespan
Liveliness

6.1 联合仿真 QoS 最佳实践

在联合仿真中，不同类型的数据应该使用不同的 QoS 策略：

6.2 QoS 不兼容问题

QoS 不兼容是联合仿真中最常见也最难以调试的问题之一。当发布端和订阅端的 QoS 不兼容时，订阅端不会收到任何消息，也不会有任何错误提示。

常见的 QoS 不兼容情况：

• 发布端使用 BestEffort，订阅端使用 Reliable
• 发布端使用 Volatile，订阅端使用 TransientLocal
• 发布端的 Depth 小于订阅端的 Depth

C++ QoS 配置示例：

// 相机图像 QoS
auto camera_qos = rclcpp::QoS(10)
.best_effort()
.durability_volatile()
.history(rclcpp::HistoryPolicy::KeepLast);

// /cmd_vel QoS
auto cmd_vel_qos = rclcpp::QoS(10)
.reliable()
.durability_volatile()
.history(rclcpp::HistoryPolicy::KeepLast);

// /tf_static QoS
auto tf_static_qos = rclcpp::QoS(10)
.reliable()
.transient_local()
.history(rclcpp::HistoryPolicy::KeepLast);

// 创建发布器
auto image_pub = ros2_bridge.create_publisher<sensor_msgs::msg::Image>(
"/camera/image_raw",
camera_qos
);
auto cmd_vel_sub = ros2_bridge.create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel",
cmd_vel_callback,
cmd_vel_qos
);

6.3 QoS 排查与调试

排查命令：

# 查看话题的详细 QoS 配置
ros2 topic info /camera/image_raw -v
ros2 topic info /scan -v
ros2 topic info /cmd_vel -v

# 查看节点的订阅器 QoS 配置
ros2 node info /rviz2
ros2 node info /controller_server

对于高带宽传感器，不建议一律使用 Reliable。图像和点云在无线网络或跨机器传输时，Reliable 可能因为重传导致延迟堆积，反而降低系统实时性。在这种情况下，BestEffort 是更好的选择。

7. 移动机器人控制闭环

移动机器人联合仿真的典型闭环如下：

Isaac Sim:
publish /clock /tf /odom /scan

ROS 2:
teleop / Nav2 / custom controller

ROS 2 publish:
/cmd_vel

Isaac Sim:
subscribe /cmd_vel
Differential Controller
Articulation Controller
wheel joints

7.1 差分控制器原理

差分控制器根据输入的线速度和角速度，计算出左右轮的目标速度。计算公式为：

v_left = (2*v – w*L) / (2*r)
v_right = (2*v + w*L) / (2*r)

其中：

• v 是机器人的线速度（m/s）
• w 是机器人的角速度（rad/s）
• L 是左右轮之间的距离（轮距，m）
• r 是轮子的半径（m）

Isaac Sim 的 Differential Controller 节点已经内置了这个计算逻辑，并且支持速度和加速度限制。

7.2 速度与加速度限制

为了避免机器人运动过于剧烈，应该在差分控制器中添加速度和加速度限制：

• maxLinearSpeed：最大线速度（m/s）
• maxAngularSpeed：最大角速度（rad/s）
• maxLinearAcceleration：最大线加速度（m/s²）
• maxAngularAcceleration：最大角加速度（rad/s²）

这些参数应该与真实机器人的物理限制一致。例如，Jetbot 的最大线速度约为 0.7m/s，最大角速度约为 2.0rad/s。

C++ 差分控制器示例：

#include <omni/isaac/wheeled_robots/controllers/DifferentialController.h>

// 创建差分控制器
auto controller = std::make_shared<DifferentialController>(
"differential_controller",
0.0325, // 轮子半径 (m)
0.1125 // 轮距 (m)
);
// 设置速度和加速度限制
controller->set_max_linear_speed(0.7);
controller->set_max_angular_speed(2.0);
controller->set_max_linear_acceleration(1.0);
controller->set_max_angular_acceleration(2.0);

// /cmd_vel 回调函数
void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
controller->forward({msg->linear.x, msg->angular.z});
}

// 在仿真循环中应用控制命令
while (simulation_app.is_running()) {
world.step(render::RenderMode::kFull);

if (world.is_playing()) {
robot->apply_action(controller->get_action());
}
}

7.3 常见问题排查

最小测试命令：

ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \\
"{linear: {x: 0.3}, angular: {z: 0.5}}"

如果 ROS 2 已经发布 /cmd_vel，但机器人不动，应检查：

工程经验是：接 Nav2 前必须先 teleop 通。teleop 都无法驱动机器人时，Nav2 一定也无法正常工作。

8. Ackermann 车辆控制

对于汽车式底盘，控制接口通常不是简单的左右轮差速，而是转角和速度。常见消息类型包括：

ackermann_msgs/msg/AckermannDriveStamped

8.1 Ackermann 转向几何

Ackermann 转向几何确保车辆在转弯时，所有车轮都绕同一个中心点旋转，从而避免轮胎滑动。内侧车轮的转角大于外侧车轮的转角，满足以下关系：

cot(δ_outer) – cot(δ_inner) = W / L

其中：

• δ_inner 是内侧车轮的转角
• δ_outer 是外侧车轮的转角
• W 是轮距（左右轮之间的距离）
• L 是轴距（前后轮之间的距离）

Isaac Sim 的 Ackermann Controller 节点已经内置了 Ackermann 转向几何的计算逻辑。

8.2 系统结构

系统结构可以是：

ROS 2 /cmd_vel
→ cmdvel_to_ackermann
→ /ackermann_cmd
→ Isaac Sim Ackermann controller
→ steering joints + wheel joints

或者直接使用支持 Ackermann 控制的 Nav2 控制器：

Nav2 ackermann_controller
→ /ackermann_cmd
→ Isaac Sim Ackermann controller

8.3 工程注意事项

Ackermann 车辆仿真需要额外关注：

如果将 Nav2 用于 Ackermann 车辆，还要确认局部规划器是否支持非完整约束，以及 footprint、运动模型和控制器参数是否合理。Nav2 的 nav2_ackermann_controller 插件专门用于 Ackermann 车辆的控制。

9. Nav2 联合仿真

Nav2 是 ROS 2 中最常用的移动机器人导航框架。Isaac Sim 接入 Nav2 时，通常提供：

/clock
/tf
/tf_static
/odom
/scan 或 /point_cloud
/map 或 SLAM 输出

Nav2 输出：

/cmd_vel

9.1 最小数据图

Isaac Sim /scan ─────────────┐
Isaac Sim /odom ─────────────┤
Isaac Sim /tf ───────────────┤
/map 或 SLAM ────────────────┤
▼
Nav2
│
▼
/cmd_vel
│
▼
Isaac Sim

9.2 核心参数配置

Nav2 的核心参数通常在 nav2_params.yaml 文件中配置：

use_sim_time: true
global_frame: map
robot_base_frame: base_link
odom_frame: odom
scan_topic: /scan
map_topic: /map

# Costmap 配置
global_costmap:
global_costmap:
ros__parameters:
resolution: 0.05
inflation_radius: 0.55
cost_scaling_factor: 10.0

local_costmap:
local_costmap:
ros__parameters:
resolution: 0.05
inflation_radius: 0.55
cost_scaling_factor: 10.0
rolling_window: true
width: 3.0
height: 3.0

9.3 生命周期节点管理

Nav2 使用 ROS 2 的生命周期节点来管理各个组件的状态。在启动 Nav2 后，需要将所有节点从 Unconfigured 状态转换到 Active 状态。

常用生命周期命令：

# 查看节点状态
ros2 lifecycle list /controller_server

# 配置节点
ros2 lifecycle set /controller_server configure

# 激活节点
ros2 lifecycle set /controller_server activate

# 停用节点
ros2 lifecycle set /controller_server deactivate

# 清理节点
ros2 lifecycle set /controller_server cleanup

在实际使用中，通常使用 nav2_bringup 包中的启动文件自动完成状态转换。

9.4 SLAM 与 Nav2 集成

在未知环境中导航时，需要同时运行 SLAM 算法和 Nav2。常用的 SLAM 算法包括：

• slam_toolbox：2D 激光 SLAM，最常用，稳定性好
• rtabmap_ros：视觉 SLAM，支持 RGB-D 和立体相机
• cartographer：Google 开发的 SLAM 算法，精度高

SLAM 算法会发布 /map 话题和 map→odom 的 TF 变换，供 Nav2 使用。

9.5 常见问题排查

Nav2 不工作的常见原因：

排查顺序：

# 1. 检查时间同步
ros2 topic echo /clock
ros2 param get /controller_server use_sim_time

# 2. 检查 TF 树
ros2 run tf2_ros tf2_echo map base_link
ros2 run tf2_ros tf2_echo odom base_link
ros2 run tf2_ros tf2_echo base_link base_scan

# 3. 检查传感器数据
ros2 topic hz /scan
ros2 topic hz /odom

# 4. 检查 Nav2 节点状态
ros2 lifecycle list /controller_server
ros2 lifecycle list /planner_server

# 5. 检查控制命令
ros2 topic echo /cmd_vel

10. 机械臂联合仿真

机械臂的核心链路如下：

Isaac Sim articulation
→ /joint_states
→ robot_state_publisher
→ /tf
→ MoveIt 2 / RViz2

MoveIt 2 / controller
→ joint trajectory / joint command
→ Isaac Sim Articulation Controller

10.1 三种控制模式对比

Isaac Sim 的 Articulation Controller 支持三种控制模式：

对于大多数机械臂应用，位置控制是最常用的模式。

10.2 动力学参数调优

关节驱动器的刚度和阻尼参数对机械臂的运动稳定性有很大影响：

• stiffness：刚度系数，决定关节对位置误差的响应。刚度越高，位置跟踪精度越高，但也越容易产生振荡。
• damping：阻尼系数，决定关节的振荡衰减。阻尼越高，振荡衰减越快，但响应速度也会变慢。

调优步骤：

10.3 最小验证

最小验证可以先用 JointState 命令控制单个关节：

ros2 topic pub /joint_command sensor_msgs/msg/JointState \\
"{name: ['joint1'], position: [0.5]}"

需要重点检查：

机械臂"飞走"或严重抖动时，通常不是 ROS 2 通信问题，而是动力学配置问题，例如惯量错误、碰撞体过复杂、关节驱动增益过高、阻尼不足或自碰撞。

11. MoveIt 2 集成

MoveIt 2 是 ROS 2 中主流机械臂运动规划平台。它通常需要：

/robot_description
/robot_description_semantic
/joint_states
/tf
/planning_scene
/controller_manager 或 trajectory action

输出通常是：

FollowJointTrajectory action

MoveIt2/ROS2 机械臂规划与控制常见接口：

/robot_description
URDF 机器人模型参数。描述机器人连杆、关节、几何、碰撞体、惯量等，是 MoveIt2 建模的基础。

/robot_description_semantic
SRDF 语义模型参数。描述规划组、末端执行器、禁用碰撞对、默认姿态等，补充 URDF 里没有的规划语义信息。

/joint_states
当前关节状态话题。发布每个关节的位置、速度、力矩，MoveIt2 用它知道机器人当前姿态。

/tf
坐标变换话题。发布各坐标系之间的动态变换，比如 base_link 到各连杆、相机、末端坐标系等。

/planning_scene
规划场景话题。包含机器人当前状态、环境障碍物、碰撞物体、允许碰撞矩阵等，用于碰撞检测和路径规划。

/controller_manager
ros2_control 的控制器管理节点。负责加载、启动、停止控制器，比如 joint_trajectory_controller。

trajectory action
轨迹执行接口，通常是 FollowJointTrajectory action。MoveIt2 规划出轨迹后，通过它把关节轨迹发送给控制器执行。

FollowJointTrajectory action
这是 ros2_control 里 joint_trajectory_controller 常用的轨迹执行 action 接口，类型通常是:control_msgs/action/FollowJointTrajectory
MoveIt2 规划完成后，会把一串关节轨迹点发送到这个 action，控制器按时间执行。它主要包含：

• goal目标轨迹，包括关节名、每个时间点的关节位置/速度/加速度等。
• feedback执行中的反馈，比如当前关节状态、期望状态、误差。
• result执行结果，比如成功、取消、路径容差超限、目标容差超限等。

常见名字类似：

/arm_controller/follow_joint_trajectory
/joint_trajectory_controller/follow_joint_trajectory

MoveIt2 负责规划轨迹，FollowJointTrajectory action 负责把轨迹交给控制器执行并返回执行状态。

11.1 两种集成方式对比

联合仿真有两种常见方式：

轻量方式

MoveIt 2
→ joint trajectory
→ Isaac Sim bridge node
→ Articulation Controller

真实机器人一致性方式

MoveIt 2
→ ros2_control
→ joint_trajectory_controller
→ Isaac Sim hardware bridge
→ Articulation Controller

如果目标是未来迁移真实机械臂，强烈建议尽早引入 ros2_control 和标准 trajectory controller。这样 MoveIt 2 的控制接口、真实机器人接口和仿真接口可以保持一致，代码可以直接迁移。

11.2 FollowJointTrajectory Action

MoveIt 2 通过 FollowJointTrajectory action 来向控制器发送轨迹命令。该 action 包含轨迹的关节位置、速度、加速度和时间戳。控制器需要在指定的时间内到达每个轨迹点。

C++ FollowJointTrajectory Action 服务器示例：

#include <rclcpp_action/rclcpp_action.hpp>
#include <control_msgs/action/follow_joint_trajectory.hpp>

using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
using GoalHandle = rclcpp_action::ServerGoalHandle<FollowJointTrajectory>;

// 创建 Action 服务器
auto action_server = rclcpp_action::create_server<FollowJointTrajectory>(
ros2_bridge.node(),
"/follow_joint_trajectory",
[](const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const FollowJointTrajectory::Goal> goal) {
RCLCPP_INFO(rclcpp::get_logger("action_server"), "Received goal request");
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
},
[](const std::shared_ptr<GoalHandle> goal_handle) {
RCLCPP_INFO(rclcpp::get_logger("action_server"), "Received cancel request");
return rclcpp_action::CancelResponse::ACCEPT;
},
[this](const std::shared_ptr<GoalHandle> goal_handle) {
// 在新线程中执行轨迹
std::thread{[this, goal_handle]() {
auto result = std::make_shared<FollowJointTrajectory::Result>();
auto trajectory = goal_handle->get_goal()->trajectory;

// 执行轨迹
for (size_t i = 0; i < trajectory.points.size(); i++) {
auto& point = trajectory.points[i];

// 设置关节目标位置
for (size_t j = 0; j < trajectory.joint_names.size(); j++) {
auto joint_name = trajectory.joint_names[j];
auto position = point.positions[j];
robot->set_joint_position_target(joint_name, position);
}

// 等待到达目标位置
rclcpp::sleep_for(std::chrono::milliseconds(
static_cast<long>(point.time_from_start.sec * 1000 + point.time_from_start.nanosec / 1000000)
));

// 检查是否被取消
if (goal_handle->is_canceling()) {
result->error_code = FollowJointTrajectory::Result::PREEMPTED;
goal_handle->canceled(result);
return;
}
}

// 轨迹执行完成
result->error_code = FollowJointTrajectory::Result::SUCCESSFUL;
goal_handle->succeed(result);
}}.detach();
}
);

12. ros2_control 在联合仿真中的角色

ros2_control 的作用是统一控制器和硬件接口。典型结构为：

controller_manager
├── joint_state_broadcaster
├── joint_trajectory_controller
└── hardware_interface

在真实机器人中，hardware_interface 连接电机驱动器；在 Isaac Sim 中，hardware_interface 可以连接 ROS 2 Bridge、自定义 Isaac Sim 控制节点或仿真内部 API。

12.1 三种工程模式对比

12.2 Isaac Sim 官方 ros2_control 集成

Isaac Sim 5.0 及以上版本提供了官方的 ros2_control 硬件接口 isaac_ros2_control。它可以直接连接到 Isaac Sim 的 Articulation Controller，无需编写额外的代码。

使用方法：

hardware_interface:
type: isaac_ros2_control/IsaacSimHardware
parameters:
articulation_prim_path: "/World/UR10"
joint_names: ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]

ros2 run controller_manager controller_manager –ros-args -p use_sim_time:=true

ros2 control load_controller joint_state_broadcaster
ros2 control load_controller joint_trajectory_controller
ros2 control set_controller_state joint_state_broadcaster active
ros2 control set_controller_state joint_trajectory_controller active

12.3 控制频率考量

如果控制频率超过数百 Hz，尤其涉及力控、接触、双臂协作或腿足机器人，建议不要把所有高频闭环都放在 ROS 2 topic 中。高频控制应尽量靠近物理仿真或硬件层，ROS 2 负责任务级和策略级命令。

例如，对于一个腿足机器人：

• 关节电流环控制：1kHz，在 Isaac Sim 内部实现
• 关节位置环控制：500Hz，在 Isaac Sim 内部实现
• 身体姿态控制：100Hz，在 Isaac Sim 内部实现
• 步态规划：50Hz，在 ROS 2 中实现
• 导航和路径规划：10Hz，在 ROS 2 中实现

这种分层控制架构可以在保证控制性能的同时，充分利用 ROS 2 的生态优势。

13. 常见接口清单

联合仿真常用 topic：

/clock rosgraph_msgs/msg/Clock
/tf tf2_msgs/msg/TFMessage
/tf_static tf2_msgs/msg/TFMessage
/joint_states sensor_msgs/msg/JointState
/odom nav_msgs/msg/Odometry
/cmd_vel geometry_msgs/msg/Twist
/scan sensor_msgs/msg/LaserScan
/point_cloud sensor_msgs/msg/PointCloud2
/camera/image_raw sensor_msgs/msg/Image
/camera/camera_info sensor_msgs/msg/CameraInfo
/imu sensor_msgs/msg/Imu
/wrench geometry_msgs/msg/WrenchStamped

机械臂常用接口：

/joint_states
/joint_command
/follow_joint_trajectory control_msgs/action/FollowJointTrajectory
/gripper_command control_msgs/action/GripperCommand
/move_group
/planning_scene
/robot_description
/robot_description_semantic

仿真控制接口：

/reset_simulation std_srvs/srv/Empty
/pause_simulation std_srvs/srv/Empty
/play_simulation std_srvs/srv/Empty
/step_simulation std_srvs/srv/Trigger
/spawn_entity gazebo_msgs/srv/SpawnEntity
/delete_entity gazebo_msgs/srv/DeleteEntity
/load_world gazebo_msgs/srv/SetPhysicsProperties

C++ 仿真控制服务示例：

#include <std_srvs/srv/empty.hpp>

// 创建 /reset_simulation 服务
auto reset_service = ros2_bridge.node()->create_service<std_srvs::srv::Empty>(
"/reset_simulation",
[&world](const std_srvs::srv::Empty::Request::SharedPtr request,
std_srvs::srv::Empty::Response::SharedPtr response) {
RCLCPP_INFO(rclcpp::get_logger("simulation_control"), "Resetting simulation");
world.reset();
}
);

// 创建 /pause_simulation 服务
auto pause_service = ros2_bridge.node()->create_service<std_srvs::srv::Empty>(
"/pause_simulation",
[&world](const std_srvs::srv::Empty::Request::SharedPtr request,
std_srvs::srv::Empty::Response::SharedPtr response) {
RCLCPP_INFO(rclcpp::get_logger("simulation_control"), "Pausing simulation");
world.pause();
}
);

// 创建 /play_simulation 服务
auto play_service = ros2_bridge.node()->create_service<std_srvs::srv::Empty>(
"/play_simulation",
[&world](const std_srvs::srv::Empty::Request::SharedPtr request,
std_srvs::srv::Empty::Response::SharedPtr response) {
RCLCPP_INFO(rclcpp::get_logger("simulation_control"), "Playing simulation");
world.play();
}
);

14. 总结

ROS 2 与 Isaac Sim 联合仿真的第二阶段核心，是让仿真系统不仅"能通信"，而且能够支撑真实机器人算法栈闭环运行。

移动机器人重点关注：

/clock 时间同步
/tf 坐标树完整性
/odom 里程计精度
/scan 激光数据质量
/cmd_vel 控制响应
Nav2 生命周期管理
Costmap 配置
QoS 兼容性

机械臂重点关注：

/joint_states 关节状态
/tf 坐标变换
joint name 一致性
joint limit 正确性
Articulation Controller 参数调优
MoveIt 2 运动规划
ros2_control 硬件接口
FollowJointTrajectory 轨迹执行

传感器重点关注：

timestamp 时间戳正确性
frame_id 坐标系一致性
QoS 服务质量
publish rate 发布频率
数据体量与带宽
下游算法兼容性
噪声模型与真实度

只有传感器、控制器、TF 和时间同步同时正确，Nav2、MoveIt 2 和自研算法栈才能在 Isaac Sim 中稳定运行。