urdf 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros2/urdf
版本：2.6.1（Humble），子包 2 个，许可证 BSD。

本仓库 不是 URDF XML 的完整解析实现，而是 ROS 2 对 统一机器人描述格式（URDF） 的 C++ 入口层：提供稳定 API urdf::Model，通过 pluginlib 选择解析插件，默认将 XML 交给 urdfdom 的 parseURDF()。数据结构与底层 DOM 解析在 urdfdom / urdfdom_headers（独立仓库 src/ros/urdfdom）中实现。

范围说明：本目录仅 2 包、约 38 个源文件；真正的 XML→Link/Joint 树构建见 urdfdom。rosconsole_bridge.cpp 存在于源码树但 未编入 CMakeLists.txt（ROS 1 遗留）。

---

1. 总体认识

1.1 核心职责

包	职责
urdf_parser_plugin	定义抽象基类 urdf::URDFParser（header-only INTERFACE 库）
urdf	urdf::Model 门面 + 默认 XML 插件 URDFXMLParser + pluginlib 调度

1.2 在 ROS 2 栈中的位置

flowchart TB
  subgraph sources [机器人描述来源]
    PARAM["robot_description 参数"]
    FILE[".urdf / xacro 展开后 XML"]
  end
  subgraph this_repo [ros2/urdf 本仓库]
    MODEL["urdf::Model\ninitString / initFile"]
    PLUG["pluginlib\nClassLoader URDFParser"]
    XMLP["urdf_xml_parser\nURDFXMLParser"]
  end
  subgraph urdfdom_repo [ros/urdfdom 外部]
    PARSE["urdf::parseURDF()"]
    MI["ModelInterface\nlinks / joints / materials"]
  end
  subgraph consumers [下游]
    RSP["robot_state_publisher"]
    RVIZ["rviz RobotModelDisplay"]
    KDL["kdl_parser / MoveIt"]
    GAZ["gazebo / ros2_control"]
  end

  PARAM --> MODEL
  FILE --> MODEL
  MODEL --> PLUG --> XMLP --> PARSE --> MI
  MODEL --> consumers
  MI --> consumers

对比项	本仓库 urdf	urdfdom
API 稳定性	承诺向后兼容（package.xml）	底层库，随 urdfdom 版本演进
解析逻辑	插件选择 + 结果拷贝	TinyXML / libxml 解析 XML
插件	支持 多格式（可扩展 COLLADA 等）	无 pluginlib
典型调用	Model::initString(xml)	parseURDF(xml) 直接

1.3 典型用法

#include "urdf/model.h"

urdf::Model model;
if (!model.initFile("/path/to/robot.urdf")) {
  // 解析失败
}
urdf::LinkConstSharedPtr root = model.getRoot();
// 遍历 model.links_ / getLink(name) / getJoint(name)

ROS 2 节点中常见模式（如 rviz RobotModelDisplay）：

std::string xml = /* 从 /robot_description 参数或 topic */;
urdf::Model descr;
descr.initString(xml);
// 将 ModelInterface 传给 Robot、KDL 等

---

2. 仓库结构

urdf/
├── README.md
├── urdf_parser_plugin/                 # ★ 插件接口（~70 行头文件）
│   ├── include/urdf_parser_plugin/parser.h
│   └── CMakeLists.txt                  # INTERFACE library
└── urdf/                               # ★ 主包
    ├── include/urdf/
    │   ├── model.h                     # 公开 API
    │   ├── visibility_control.hpp
    │   └── urdfdom_compatibility.h     # 构建时生成
    ├── src/
    │   ├── model.cpp                   # Model::initString 核心
    │   ├── urdf_plugin.cpp             # URDFXMLParser 插件
    │   └── rosconsole_bridge.cpp       # 未链接（遗留）
    ├── urdf_parser_description.xml     # pluginlib 注册
    ├── urdfdom_compatibility.h.in
    ├── CMakeLists.txt
    └── test/                           # pr2 等 URDF 样例 + gtest

---

3. urdf_parser_plugin：插件抽象

3.1 URDFParser 接口

urdf_parser_plugin/include/urdf_parser_plugin/parser.h：

class URDFParser {
public:
  virtual urdf::ModelInterfaceSharedPtr parse(const std::string & data) = 0;
  virtual size_t might_handle(const std::string & data) = 0;
};

方法	语义
might_handle(data)	返回「置信度分数」：越小越自信 能解析该字符串
parse(data)	解析成功返回 ModelInterfaceSharedPtr；失败返回 nullptr

置信度约定（文档注释）：

• 若数据 不属于 本插件：返回 >= data.size()
• 若属于：返回格式特征在字符串中的 位置（如 <robot 首次出现下标）
• 例：COLLADA 插件可返回 <COLLADA> 标签位置

3.2 CMake 导出

add_library(urdf_parser_plugin INTERFACE)
ament_target_dependencies(urdf_parser_plugin INTERFACE urdfdom_headers)

仅依赖 urdfdom_headers（类型前向声明），不链接 urdfdom 实现，便于第三方写轻量插件。

---

4. urdf::Model：插件调度门面

4.1 类设计

urdf::Model 继承 urdf::ModelInterface（urdfdom 定义的数据容器）：

• 公开：initFile()、initString()
• 私有：PIMPL ModelImplementation 持有 pluginlib::ClassLoader<urdf::URDFParser>

class ModelImplementation {
public:
  ModelImplementation()
  : loader_("urdf_parser_plugin", "urdf::URDFParser") {}

  pluginlib::ClassLoader<urdf::URDFParser> loader_;
};

pluginlib 在包 urdf_parser_plugin 上查找基类 urdf::URDFParser 的插件（注意：loader 第一个参数是 导出插件的包名，不是 urdf）。

4.2 initFile()

1. std::fstream 读 entire file 到 xml_string
2. 调用 initString(xml_string)

4.3 initString() — 插件选择算法

核心逻辑（model.cpp）：

best_score = SIZE_MAX
for each plugin_name in loader_.getDeclaredClasses():
    plugin = load_plugin(plugin_name)
    score = plugin->might_handle(data)
    if score < best_score:
        best_score = score
        best_plugin = plugin

if best_score >= data.size():
    // 无插件足够自信 → 强制尝试默认 URDF XML 插件
    best_plugin = load("urdf_xml_parser/URDFXMLParser")

model = best_plugin->parse(data)
if model:
    拷贝 links_, joints_, materials_, name_, root_link_ 到 this

要点：

1. 自动选举：所有已注册插件参与 might_handle 竞标
2. 兜底：若无插件 score < data.size()，硬编码加载 urdf_xml_parser/URDFXMLParser
3. 无法指定插件：注释写明除卸载插件外 无法覆盖 自动选择
4. 失败路径：fprintf(stderr, ...) 输出错误（非 rclcpp 日志）

4.4 与 ModelInterface 的关系

解析成功后 浅拷贝 成员到当前 Model 对象：

this->links_ = model->links_;
this->joints_ = model->joints_;
this->materials_ = model->materials_;
this->name_ = model->name_;
this->root_link_ = model->root_link_;

ModelInterface 提供 getRoot()、getLink()、getJoint() 等树访问 API（定义在 urdfdom_headers）。

---

5. 默认插件：URDFXMLParser

5.1 实现

urdf/src/urdf_plugin.cpp（编译为 liburdf_xml_parser.so）：

class URDFXMLParser final : public urdf::URDFParser {
  urdf::ModelInterfaceSharedPtr parse(const std::string & xml_string) override {
    return urdf::parseURDF(xml_string);  // urdfdom
  }

  size_t might_handle(const std::string & data) override {
    tinyxml2::XMLDocument doc;
    if (doc.Parse(data.c_str()) == tinyxml2::XML_SUCCESS) {
      if (std::string("robot") != doc.RootElement()->Name())
        return data.size();  // XML 但非 robot
    }
    return data.find("<robot");  // 越靠前越自信
  }
};

PLUGINLIB_EXPORT_CLASS(urdf::URDFXMLParser, urdf::URDFParser)

依赖：

• urdfdom — parseURDF()
• tinyxml2 — 仅用于 might_handle 快速探测根元素

5.2 pluginlib 注册

urdf/urdf_parser_description.xml：

<library path="urdf_xml_parser">
  <class name="urdf_xml_parser/URDFXMLParser"
         type="urdf::URDFXMLParser"
         base_class_type="urdf::URDFParser">
    <description>Parse models as URDF from URDF XML.</description>
  </class>
</library>

CMake：

pluginlib_export_plugin_description_file(urdf_parser_plugin "urdf_parser_description.xml")

---

6. urdfdom 层（外部依赖）

实际 XML 解析在 /home/cp/work2/ros2Learn/ros2_humble/src/ros/urdfdom：

组件	内容
urdfdom_headers	Link、Joint、Geometry（box/cylinder/mesh/sphere）、Material 等 C++ 类型
urdfdom	parseURDF()、parseURDFFile()、TinyXML/libxml 解析、树校验

parseURDF() 概要流程：

1. 创建 ModelInterface
2. 解析 XML <robot name="...">
3. 填充 links_、joints_ map，建立 root_link_
4. 校验 joint 父子关系、无环、material 引用等
5. 失败返回 nullptr

本仓库 不包含 上述逻辑；benchmark 测试直接与 parseURDF 对比插件开销。

---

7. urdfdom_compatibility.h

构建时由 urdfdom_compatibility.h.in + configure_file 生成，解决 urdfdom 0.4 前后 shared_ptr 类型差异：

urdfdom_headers 版本	行为
<= 0.4	使用 boost::shared_ptr，宏 URDF_TYPEDEF_CLASS_POINTER 批量 typedef
> 0.4	使用 std::shared_ptr，定义 ModelSharedPtr 等

Humble 使用 ROS 2 打包的 urdfdom_headers（≥ 1.0），走 std::shared_ptr 分支。

---

8. 构建产物

目标	类型	说明
liburdf.so	SHARED	Model 实现（model.cpp）
liburdf_xml_parser.so	SHARED	默认 URDF XML 插件
urdf_parser_plugin	INTERFACE	仅头文件导出

导出依赖（ament_export_dependencies）：pluginlib、urdf_parser_plugin、urdfdom、urdfdom_headers

---

9. 自定义解析插件

若需支持 非 URDF XML 格式（历史上曾有 COLLADA 机器人描述等）：

1. 新建包，依赖 urdf_parser_plugin、urdfdom
2. 继承 urdf::URDFParser，实现 parse() + might_handle()
3. PLUGINLIB_EXPORT_CLASS(..., urdf::URDFParser)
4. 编写 plugin_description.xml，base_class_type="urdf::URDFParser"
5. pluginlib_export_plugin_description_file(urdf_parser_plugin "your_plugins.xml")

注意：loader 扫描的是 urdf_parser_plugin 包名下的插件；description 文件必须 export 到该 plugin 包关联 index。

might_handle 设计要点：

• 对 URDF XML 返回 data.find("<robot") 通常较小
• 自定义格式应返回 更小的 score 才能赢得选举
• 错误返回 data.size() 表示不参与

---

10. 测试

10.1 test_robot_model_parser.cpp

• 使用 urdf::Model::initFile 加载 test/pr2_desc.urdf 等
• 递归 traverse_tree 验证 link/joint 数量与结构
• 多个 fail_*.urdf 负例（双树、环、缺 joint 等）

10.2 benchmark_plugin_overhead.cpp

• Google Benchmark 对比：
  • 直接 urdf::parseURDF(test_xml)
  • urdf::Model::initString(test_xml)（含 plugin 选举 + 拷贝）
• 使用 pluginlib_enable_plugin_testing mock 安装环境

10.3 urdfdom_compatibility.cpp

• 验证生成的 compatibility 头与 urdfdom 版本匹配

---

11. 下游消费者（ROS 2）

包/组件	使用方式
robot_state_publisher	读 robot_description → urdf::Model → 发布 TF
rviz2 RobotModelDisplay	initString(robot_description) → Ogre 网格
kdl_parser	Model → KDL Tree
MoveIt / ros2_control	运动学、碰撞模型
gazebo / Ignition	生成仿真模型

数据流共性：

XML string → urdf::Model → ModelInterface (links/joints)
         → KDL / RBDL / 可视化 / 碰撞检测

xacro：本仓库 不处理 xacro；通常由 launch 或 xacro 命令先展开为 XML，再 initString。

---

12. 序列图：initString 完整路径

---

13. URDF 数据模型概要

解析成功后 ModelInterface 包含（urdfdom_headers）：

类型	说明
Link	连杆：inertial、visual、collision、子 joint 列表
Joint	关节：type（revolute/prismatic/fixed/…）、axis、limit、dynamics、parent/child link
Material	颜色、纹理
Geometry	box / cylinder / sphere / mesh（含 filename、scale）

树结构：单 root link；joint 连接 parent→child，形成 kinematic tree（不允许环）。

---

14. 调试与常见问题

现象	排查
Could not open file	initFile 路径错误
Failed to parse robot description using: ...	XML 语法或 URDF 语义错误；用 check_urdf（若安装）
No plugin found	pluginlib index 缺失；确认 liburdf_xml_parser.so 在 AMENT_PREFIX_PATH
插件未选中	检查其他插件 might_handle 是否返回更小 score
shared_ptr 编译错误	urdfdom_headers 与 urdfdom_compatibility.h 版本不匹配
xacro 未展开	应对字符串先运行 xacro，再 initString

命令：

# 检查 URDF（来自 liburdfdom-tools，非本仓库）
check_urdf robot.urdf
# 参数中的描述
ros2 param get /robot_state_publisher robot_description

---

15. 与 ROS 1 robot_model 仓库关系

• 原属 ros/robot_model，后拆至 ros2/urdf（见 README issue #195）
• ROS 2 revival 了 可插拔 parser（urdf_parser_plugin + pluginlib）
• API 保持 urdf::Model::initString/initFile 向后兼容承诺

---

16. 源码阅读顺序

1. 插件接口：urdf_parser_plugin/parser.h
2. 公开 API：urdf/model.h
3. 调度核心：urdf/src/model.cpp 的 initString()
4. 默认插件：urdf/src/urdf_plugin.cpp
5. 注册：urdf_parser_description.xml + CMakeLists.txt 中 pluginlib_export_*
6. 兼容层：urdfdom_compatibility.h.in
7. 底层解析（外部）：urdfdom/urdf_parser/src/model.cpp 的 parseURDF()
8. 集成示例：rviz_default_plugins/.../robot_model_display.cpp
9. 测试：test/test_robot_model_parser.cpp

---

17. 小结

ros2/urdf 仓库体量小但位置关键：它是 ROS 2 中 加载机器人描述的标准 C++ 入口。urdf::Model 通过 pluginlib 竞标 选择 URDFParser，默认 URDFXMLParser 委托 urdfdom 完成 XML 解析；urdf_parser_plugin 仅定义插件契约。

理解机器人模型加载问题，应区分三层：本仓库（选举+API）→ urdfdom（解析+校验）→ 下游（KDL/TF/可视化）。扩展新格式只需新增 URDFParser 插件并正确实现 might_handle 评分逻辑。