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 典型用法
1
2
3
4
5
6
7
8
| #include "urdf/model.h"
urdf::Model model;
if (!model.initFile("/path/to/robot.urdf")) {
}
urdf::LinkConstSharedPtr root = model.getRoot();
|
ROS 2 节点中常见模式（如 rviz RobotModelDisplay）：
1
2
3
4
| std::string xml = ;
urdf::Model descr;
descr.initString(xml);
|
2. 仓库结构
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
| 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：
1
2
3
4
5
| 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 导出
1
2
| 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>
1
2
3
4
5
6
7
| 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()
std::fstream 读 entire file 到 xml_string- 调用
initString(xml_string)
4.3 initString() — 插件选择算法
核心逻辑（model.cpp）：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
| 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
|
要点：
- 自动选举：所有已注册插件参与
might_handle 竞标
- 兜底：若无插件 score
< data.size()，硬编码加载 urdf_xml_parser/URDFXMLParser
- 无法指定插件：注释写明除卸载插件外 无法覆盖 自动选择
- 失败路径：
fprintf(stderr, ...) 输出错误（非 rclcpp 日志）
4.4 与 ModelInterface 的关系
解析成功后 浅拷贝 成员到当前 Model 对象：
1
2
3
4
5
| 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）：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
| class URDFXMLParser final : public urdf::URDFParser {
urdf::ModelInterfaceSharedPtr parse(const std::string & xml_string) override {
return urdf::parseURDF(xml_string);
}
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();
}
return data.find("<robot");
}
};
PLUGINLIB_EXPORT_CLASS(urdf::URDFXMLParser, urdf::URDFParser)
|
依赖：
- urdfdom —
parseURDF()
- tinyxml2 — 仅用于
might_handle 快速探测根元素
5.2 pluginlib 注册
urdf/urdf_parser_description.xml：
1
2
3
4
5
6
7
| <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：
1
| 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() 概要流程：
- 创建
ModelInterface
- 解析 XML
<robot name="...">
- 填充
links_、joints_ map，建立 root_link_
- 校验 joint 父子关系、无环、material 引用等
- 失败返回
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 机器人描述等）：
- 新建包，依赖
urdf_parser_plugin、urdfdom
- 继承
urdf::URDFParser，实现 parse() + might_handle()
PLUGINLIB_EXPORT_CLASS(..., urdf::URDFParser)- 编写
plugin_description.xml，base_class_type="urdf::URDFParser"
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 |
生成仿真模型 |
数据流共性：
1
2
| 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 |
命令：
1
2
3
4
|
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. 源码阅读顺序
- 插件接口：
urdf_parser_plugin/parser.h
- 公开 API：
urdf/model.h
- 调度核心：
urdf/src/model.cpp 的 initString()
- 默认插件：
urdf/src/urdf_plugin.cpp
- 注册：
urdf_parser_description.xml + CMakeLists.txt 中 pluginlib_export_*
- 兼容层：
urdfdom_compatibility.h.in
- 底层解析（外部）：
urdfdom/urdf_parser/src/model.cpp 的 parseURDF()
- 集成示例：
rviz_default_plugins/.../robot_model_display.cpp
- 测试：
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 评分逻辑。