2026-09-05

ROS2►ROS

urdfdom 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros/urdfdom
版本：3.0.2，许可证 BSD。

urdfdom 是 URDF（Unified Robot Description Format）的 XML 解析与序列化库：用 TinyXML 读取 URDF XML，填充 urdfdom_headers 中定义的 C++ 数据结构（Link、Joint、Material 等）。它本身不做运动学计算，也不依赖 rclcpp；ROS 2 中 urdf 包、kdl_parser、robot_state_publisher 等都在此之上工作。

1. 与 urdfdom_headers 的分工

urdfdom 与同级包 urdfdom_headers 是配套关系：

包	职责	内容
urdfdom_headers	数据模型（头文件）	`ModelInterface`、`Link`、`Joint`、`Pose`、`Inertial`…
urdfdom	XML ↔ 数据模型	`parseURDF()`、`parseLink()`、`exportURDF()`…

URDF XML 文件
    ↓ TinyXML 解析 (urdfdom)
urdf::ModelInterface (urdfdom_headers)
    ↓
urdf::Model (ros2/urdf) / kdl_parser / Gazebo / MoveIt ...

2. 仓库结构

urdfdom/
├── CMakeLists.txt                 # 顶层构建、依赖、安装
├── package.xml
├── cmake/                         # FindTinyXML、pkg-config、config.cmake
├── xsd/
│   ├── urdf.xsd                   # URDF XML Schema
│   └── autogenerated.urdf
└── urdf_parser/
    ├── include/urdf_parser/
    │   ├── urdf_parser.h          # 公共 API
    │   └── exportdecl.h           # DLL 导出宏
    ├── src/
    │   ├── model.cpp              # ★ parseURDF 主流程
    │   ├── link.cpp               # link/visual/collision/inertial
    │   ├── joint.cpp              # joint 全类型 + mimic/limit/dynamics
    │   ├── pose.cpp               # origin xyz/rpy
    │   ├── world.cpp              # world 解析（桩）
    │   ├── urdf_sensor.cpp        # camera/ray 传感器
    │   ├── urdf_model_state.cpp   # 模型状态
    │   ├── twist.cpp
    │   ├── check_urdf.cpp         # CLI 校验工具
    │   └── urdf_to_graphviz.cpp   # 导出 Graphviz DOT
    └── test/                      # gtest + 内嵌 gtest 源码

3. 四个共享库 + 一个 INTERFACE 目标

add_urdfdom_library(LIBNAME urdfdom_world
  SOURCES src/pose.cpp src/model.cpp src/link.cpp src/joint.cpp src/world.cpp)

add_urdfdom_library(LIBNAME urdfdom_model
  SOURCES src/pose.cpp src/model.cpp src/link.cpp src/joint.cpp)

add_urdfdom_library(LIBNAME urdfdom_sensor
  SOURCES src/urdf_sensor.cpp
  LINK urdfdom_model)

add_urdfdom_library(LIBNAME urdfdom_model_state
  SOURCES src/urdf_model_state.cpp src/twist.cpp)

add_library(urdf_parser INTERFACE)
target_link_libraries(urdf_parser INTERFACE urdfdom_model urdfdom_sensor urdfdom_world)

库	内容	ROS 2 常用程度
`liburdfdom_model.so`	robot/link/joint 核心解析	最常用
`liburdfdom_world.so`	含 world 桩代码	很少
`liburdfdom_sensor.so`	`<sensor>` 扩展	较少
`liburdfdom_model_state.so`	模型状态快照	较少
`urdf_parser` (INTERFACE)	聚合上述库	CMake 依赖用

注意：model.cpp、link.cpp、joint.cpp、pose.cpp 在 urdfdom_model 与 urdfdom_world 中各编译一份（代码重复链接到两个 .so）。

4. 公共 API

namespace urdf{
  URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string);
  URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string &path);
  URDFDOM_DLLAPI TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model);
  URDFDOM_DLLAPI TiXmlDocument* exportURDF(const ModelInterface &model);
  URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*);
  ...
}

函数	作用
`parseURDF(xml)`	从 XML 字符串解析，成功返回 `ModelInterfaceSharedPtr`，失败返回 null
`parseURDFFile(path)`	读文件后调用 `parseURDF`
`exportURDF(model)`	将内存模型序列化为 `TiXmlDocument`
`URDFVersion`	解析 `<robot version="x.y">`，当前仅支持 1.0

5. `parseURDF` 主流程

ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
{
  ModelInterfaceSharedPtr model(new ModelInterface);
  TiXmlDocument xml_doc;
  xml_doc.Parse(xml_string.c_str());
  ...
  TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
  model->name_ = robot_xml->Attribute("name");
  // 版本必须为 1.0
  URDFVersion version(robot_xml->Attribute("version"));

  // 1. 解析所有 <material>
  // 2. 解析所有 <link>，关联 visual 材质
  // 3. 解析所有 <joint>
  // 4. model->initTree(parent_link_tree)  建立父子关系
  // 5. model->initRoot(parent_link_tree)  确定唯一根 link
  return model;
}

解析顺序严格为：material → link → joint → 建树。

失败策略：任一步出错即 model.reset() 返回 null，错误通过 console_bridge 打日志。

6. 建树逻辑（urdfdom_headers）

initTree 与 initRoot 定义在 urdfdom_headers/include/urdf_model/model.h：

void initTree(std::map<std::string, std::string> &parent_link_tree)
{
  for (each joint) {
    // child_link->setParent(parent_link)
    // child_link->parent_joint = joint
    // parent_link->child_links.push_back(child_link)
    parent_link_tree[child_name] = parent_name;
  }
}

void initRoot(const std::map<std::string, std::string> &parent_link_tree)
{
  // 找 parent_link_tree 中不出现的 link → 根
  // 必须恰好一个根，否则 ParseError
}

约束：

每个 joint 必须有 parent/child link，且 link 必须已定义
整棵 URDF 必须是单根树（一个 root link）
多根或无根都会抛 ParseError

7. Link 解析（`link.cpp`）

parseLink 支持：

XML 元素	C++ 结构	说明
`<inertial>`	`Inertial`	mass、6 惯性张量分量、origin
`<visual>` × N	`visual_array[]`	几何 + 材质引用
`<collision>` × N	`collision_array[]`	碰撞几何
几何类型	`Geometry`	sphere / box / cylinder / mesh

几何解析示例（mesh）：

bool parseMesh(Mesh &m, TiXmlElement *c)
{
  m.filename = c->Attribute("filename");
  if (c->Attribute("scale"))
    m.scale.init(c->Attribute("scale"));
  else
    m.scale = (1,1,1);
}

材质处理两阶段：

顶层 <material name="..."> 进入 model->materials_
visual 中 <material name="Grey"/> 通过 assignMaterial() 解析引用或内联定义

8. Joint 解析（`joint.cpp`）

支持的关节类型：

XML type	`Joint::Type`
fixed	FIXED
revolute	REVOLUTE（必须有 `<limit>`）
continuous	CONTINUOUS
prismatic	PRISMATIC（必须有 `<limit>`）
floating	FLOATING
planar	PLANAR

可选子元素：

元素	结构	要点
`<origin>`	`parent_to_joint_origin_transform`	缺省为单位变换
`<axis xyz="...">`	`Vector3`	缺省 (1,0,0)
`<limit>`	`JointLimits`	effort/velocity 必填
`<mimic joint="..." multiplier="" offset="">`	`JointMimic`	robot_state_publisher 会用到
`<dynamics>`	damping/friction
`<safety_controller>`	soft limits + k gains
`<calibration>`	rising/falling

9. Pose 解析（`pose.cpp`）

bool parsePose(Pose &pose, TiXmlElement* xml)
{
  if (xml->Attribute("xyz"))
    pose.position.init(xyz_str);
  if (xml->Attribute("rpy"))
    pose.rotation.init(rpy_str);  // roll-pitch-yaw
}

<origin xyz="..." rpy="..."/> 在 joint、visual、collision、inertial 中广泛使用。数值解析与异常类型（ParseError）在 urdfdom_headers 的 utils.h 中实现。

10. 扩展模块状态

模块	文件	状态
World	`world.cpp`	`parseWorld()` 为桩，”to be implemented”
Sensor	`urdf_sensor.cpp`	实现 camera/ray 等 `<sensor>` 解析
ModelState	`urdf_model_state.cpp`	机器人状态快照格式

大多数 ROS 2 机器人描述只需 urdfdom_model。

11. 命令行工具

工具	作用
`check_urdf`	解析 URDF，打印 robot 名与 link 树
`urdf_to_graphviz`	输出 Graphviz DOT（`urdf_to_graphiz` 为拼写错误的 deprecated 别名）
`urdf_mem_test`	内存/解析压力测试

check_urdf 用法：

1 2	check_urdf robot.urdf # 输出 root link 及递归 child 树

12. 依赖关系

urdfdom
  ├── urdfdom_headers   # C++ 数据模型（header-only 安装）
  ├── TinyXML           # XML 解析（v1，tinyxml.h）
  ├── tinyxml_vendor    # ROS 提供的 TinyXML 封装
  └── console_bridge    # CONSOLE_BRIDGE_logError/Debug

构建类型为 纯 cmake（非 ament），ROS 2 通过 package.xml 的 <build_type>cmake</build_type> 集成进 colcon。

13. 在 ROS 2 中的调用链

ROS 2 应用通常不直接调用 urdfdom，而是走 urdf 包：

urdf::URDFXMLParser 内部直接调用：

urdf::ModelInterfaceSharedPtr URDFXMLParser::parse(const std::string & xml_string)
{
  return urdf::parseURDF(xml_string);
}

robot_state_publisher 中的典型用法：

1
2
3

urdf::Model model;
model.initString(urdf_xml);
kdl_parser::treeFromUrdfModel(model, tree);

14. 测试

测试	内容
`urdf_unit_test`	Rotation RPY/四元数往返、URDF 解析边界
`urdf_version_test`	version 属性校验
`urdf_double_convert`	浮点转换
`memtest`	大规模解析内存

测试内嵌了完整 gtest 源码（较老的做法）。

15. XSD 与规范

xsd/urdf.xsd 提供 URDF XML Schema；autogenerated.urdf 为生成示例。运行时解析不强制 XSD 校验，靠手写 TinyXML 逻辑 + 运行时检查。

当前硬编码限制：

if (!version.equal(1, 0))
{
  throw std::runtime_error("Invalid 'version' specified; only version 1.0 is currently supported");
}

16. 设计特点与局限

特点	说明
DOM 风格	解析后完整对象树在内存，可随机访问
双向转换	parse + export（round-trip 基本支持）
树结构校验	initTree/initRoot 保证单根 kinematic tree
console_bridge 日志	错误/调试信息统一输出
多库拆分	按功能域分 so，可按需链接

局限	说明
TinyXML v1	较老 API，非 TinyXML2
仅 URDF 1.0	version 属性其他值拒绝
World 未实现	`parseWorld` 为空
无 xacro	xacro 由 launch/外部工具预处理
无 schema 运行时验证	依赖代码内检查
代码重复	model/link/joint 源文件编入两个 .so

17. 推荐阅读顺序

数据模型：urdfdom_headers/include/urdf_model/model.h、link.h、joint.h
入口：model.cpp 的 parseURDF()
细节：link.cpp（几何/惯性）→ joint.cpp（关节类型/mimic）→ pose.cpp
集成：ros2/urdf/urdf/src/urdf_plugin.cpp → model.cpp（pluginlib 选择）
下游：kdl_parser / robot_state_publisher 如何把 ModelInterface 转为 KDL/TF
工具：check_urdf.cpp 理解解析结果树形结构

18. 小结

urdfdom 是 ROS 机器人描述的 XML 解析层：TinyXML 读入 URDF，填充 urdfdom_headers 的 ModelInterface，并通过 initTree/initRoot 建立 link-joint 树。ROS 2 中 urdf::Model::initString() 经 pluginlib 默认调用 urdf::parseURDF()，再被 kdl_parser、robot_state_publisher、rviz 等广泛使用。理解 urdfdom 是理解整条 URDF → 运动学 → TF/可视化 链路的起点。

如果你希望，我可以把本文写入 ros2doc/ros/urdfdom 源码详细分析.md，或继续分析 urdfdom_headers 中 Link/Joint 数据结构的完整字段定义。

文章互动

阅读 --

留言

0 条留言

正在加载留言…

fishBlog

专注嵌入式开发

urdfdom 源码详细分析

urdfdom 源码详细分析

1. 与 urdfdom_headers 的分工

2. 仓库结构

3. 四个共享库 + 一个 INTERFACE 目标

4. 公共 API

5. `parseURDF` 主流程

6. 建树逻辑（urdfdom_headers）

7. Link 解析（`link.cpp`）

8. Joint 解析（`joint.cpp`）

9. Pose 解析（`pose.cpp`）

10. 扩展模块状态

11. 命令行工具

12. 依赖关系

13. 在 ROS 2 中的调用链

14. 测试

15. XSD 与规范

16. 设计特点与局限

17. 推荐阅读顺序

18. 小结

文章互动

留言

urdfdom 源码详细分析

1. 与 urdfdom_headers 的分工

2. 仓库结构

3. 四个共享库 + 一个 INTERFACE 目标

4. 公共 API

5. parseURDF 主流程

6. 建树逻辑（urdfdom_headers）

7. Link 解析（link.cpp）

8. Joint 解析（joint.cpp）

9. Pose 解析（pose.cpp）

10. 扩展模块状态

11. 命令行工具

12. 依赖关系

13. 在 ROS 2 中的调用链

14. 测试

15. XSD 与规范

16. 设计特点与局限

17. 推荐阅读顺序

18. 小结

文章互动

留言

5. `parseURDF` 主流程

7. Link 解析（`link.cpp`）

8. Joint 解析（`joint.cpp`）

9. Pose 解析（`pose.cpp`）