ROS2►ROS

robot_state_publisher 源码详细分析

robot_state_publisher 源码详细分析

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

robot_state_publisher 是 ROS 2 中连接 URDF 运动学模型 与 tf2 坐标系树 的核心节点：启动时加载 URDF，订阅 joint_states，用 KDL 计算各 link 相对位姿并发布到 /tf 与 /tf_static。rviz、MoveIt、导航栈等都依赖它提供的 TF 树。

1. 仓库结构

1
2
3
4
5
6
7
8
9
10
11
robot_state_publisher/
├── include/robot_state_publisher/
│   └── robot_state_publisher.hpp    # RobotStatePublisher 类声明
├── src/
│   └── robot_state_publisher.cpp      # 全部实现（~428 行）
├── launch/                            # URDF/xacro 加载示例（8 个 launch 文件）
├── urdf/                              # 示例 URDF
├── test/                              # launch_testing + gtest 集成测试
├── CMakeLists.txt
├── package.xml
└── README.md

特点：单节点、单源文件；通过 rclcpp_components 注册为可组合组件。

2. 在 ROS 2 栈中的位置

flowchart TB
  subgraph input [输入]
    URDF["robot_description 参数\n(URDF XML)"]
    JS["/joint_states\nsensor_msgs/JointState"]
  end

  subgraph rsp [robot_state_publisher]
    PARSE["urdf::Model + kdl_parser"]
    SEG["segments_ / segments_fixed_"]
    FK["KDL::Segment::pose(q)"]
    TF["TransformBroadcaster"]
    STF["StaticTransformBroadcaster"]
  end

  subgraph output [输出]
    RD["/robot_description\nstd_msgs/String"]
    TFOUT["/tf"]
    STFOUT["/tf_static"]
  end

  subgraph consumers [消费者]
    RVIZ[rviz]
    MOVEIT[MoveIt]
    NAV[nav2 / slam]
  end

  URDF --> PARSE --> SEG
  JS --> FK
  SEG --> FK
  FK --> TF --> TFOUT
  SEG --> STF --> STFOUT
  URDF --> RD
  TFOUT --> consumers
  STFOUT --> consumers
角色 说明
上游 launch 文件设置 robot_description；joint_state_publisher / 控制器发布关节角
本包 URDF → KDL 树 → 逐关节 TF
下游 tf2 监听者（rviz、MoveIt、AMCL 等）

3. 类设计

3.1 SegmentPair

1
2
3
4
5
6
7
8
9
10
11
12
13
class SegmentPair final
{
public:
  explicit SegmentPair(
    const KDL::Segment & p_segment,
    const std::string & p_root,
    const std::string & p_tip)
  : segment(p_segment), root(p_root), tip(p_tip) {}

  KDL::Segment segment;
  std::string root;  ///< 父 link 名
  std::string tip;   ///< 子 link 名
};

每个可动/固定关节对应一条 TF 边：root → tip，几何由 KDL Segment 描述。

3.2 RobotStatePublisher 核心成员

成员 类型 作用
segments_ map<string, SegmentPair> 可动关节（revolute/continuous/prismatic 等）
segments_fixed_ map<string, SegmentPair> 固定关节
mimic_ MimicMap mimic 关节映射
tf_broadcaster_ TransformBroadcaster 发布 /tf
static_tf_broadcaster_ StaticTransformBroadcaster 发布 /tf_static
description_pub_ Publisher<String> 转发 URDF 文本
joint_state_sub_ Subscription<JointState> 订阅 /joint_states
last_publish_time_ map<string, Time> 各关节上次发布时间（节流）

4. 启动与参数

4.1 构造函数流程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
RobotStatePublisher::RobotStatePublisher(const rclcpp::NodeOptions & options)
: rclcpp::Node("robot_state_publisher", options)
{
  std::string urdf_xml = this->declare_parameter("robot_description", std::string(""));
  // 空则尝试从命令行读 URDF 文件（已废弃）
  ...
  double publish_freq = this->declare_parameter("publish_frequency", 20.0);
  this->declare_parameter("frame_prefix", "");
  this->declare_parameter("ignore_timestamp", false);

  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
  static_tf_broadcaster_ = std::make_unique<tf2_ros::StaticTransformBroadcaster>(this);

  description_pub_ = this->create_publisher<std_msgs::msg::String>(
    "robot_description", rclcpp::QoS(1).transient_local());

  setupURDF(urdf_xml);

  joint_state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
    "joint_states", rclcpp::SensorDataQoS(), ...);

  publishFixedTransforms();
  // 参数回调
  param_cb_ = add_on_set_parameters_callback(...);
  parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event(...);
}

4.2 参数一览

参数 类型 默认 说明
robot_description string 必填 URDF XML 全文
publish_frequency double 20.0 可动 TF 最大发布频率 (Hz)，范围 0–1000
frame_prefix string "" 所有 frame_id 前缀（多机器人场景）
ignore_timestamp bool false true 时忽略时间戳节流，每条 joint_states 都发布

4.3 话题

方向 话题 类型 QoS
发布 robot_description std_msgs/String transient_local
发布 /tf tf2_msgs/TFMessage 默认
发布 /tf_static tf2_msgs/TFMessage transient_local
订阅 joint_states sensor_msgs/JointState SensorDataQoS

5. URDF 解析与 KDL 树构建

5.1 parseURDF

1
2
3
4
5
6
7
8
9
10
11
KDL::Tree RobotStatePublisher::parseURDF(const std::string & urdf_xml, urdf::Model & model)
{
  if (!model.initString(urdf_xml)) {
    throw std::runtime_error("Unable to initialize urdf::model from robot description");
  }
  KDL::Tree tree;
  if (!kdl_parser::treeFromUrdfModel(model, tree)) {
    throw std::runtime_error("Failed to extract kdl tree from robot description");
  }
  return tree;
}

两步转换：URDF XML → urdf::Model → KDL::Tree（依赖 kdl_parser + orocos_kdl）。

5.2 setupURDF

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
void RobotStatePublisher::setupURDF(const std::string & urdf_xml)
{
  urdf::Model model;
  KDL::Tree tree = parseURDF(urdf_xml, model);

  // 构建 mimic 映射（显式拷贝 JointMimic，避免悬空引用）
  mimic_.clear();
  for (...) {
    if (i.second->mimic) {
      auto jm = std::make_shared<urdf::JointMimic>();
      jm->offset = i.second->mimic->offset;
      jm->multiplier = i.second->mimic->multiplier;
      jm->joint_name = i.second->mimic->joint_name;
      mimic_[i.first] = jm;
    }
  }

  segments_.clear();
  segments_fixed_.clear();
  addChildren(model, tree.getRootSegment());

  // 发布 URDF 到 /robot_description
  description_pub_->publish(...);
}

5.3 addChildren：关节分类

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
void RobotStatePublisher::addChildren(...)
{
  const std::string & root = GetTreeElementSegment(segment->second).getName();
  for (children ...) {
    const KDL::Segment & child = GetTreeElementSegment(children[i]->second);
    SegmentPair s(..., root, child.getName());
    if (child.getJoint().getType() == KDL::Joint::None) {
      if (model.getJoint(...) && ...->type == urdf::Joint::FLOATING) {
        // 浮动关节：不加入任何 map
      } else {
        segments_fixed_.insert(...);  // 固定关节 → tf_static
      }
    } else {
      segments_.insert(...);  // 可动关节 → /tf
    }
    addChildren(model, children[i]);
  }
}

分类规则：

URDF/KDL 关节类型 去向 TF 话题
fixed segments_fixed_ /tf_static
revolute / continuous / prismatic segments_ /tf
floating 跳过 不发布
mimic 不单独分类 运行时从被 mimic 关节推导

kdl_parser 会把 fixed 关节映射为 KDL::Joint::None，可动关节保留 1-DOF。

6. TF 发布核心

6.1 KDL → geometry_msgs

1
2
3
4
5
6
7
8
geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame & k)
{
  geometry_msgs::msg::TransformStamped t;
  t.transform.translation.x = k.p.x();
  ...
  k.M.GetQuaternion(t.transform.rotation.x, ...);
  return t;
}

6.2 可动关节：publishTransforms

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
void RobotStatePublisher::publishTransforms(
  const std::map<std::string, double> & joint_positions,
  const builtin_interfaces::msg::Time & time)
{
  std::string frame_prefix = get_parameter("frame_prefix").get_value<std::string>();
  for (const auto & jnt : joint_positions) {
    auto seg = segments_.find(jnt.first);
    if (seg != segments_.end()) {
      geometry_msgs::msg::TransformStamped tf_transform =
        kdlToTransform(seg->second.segment.pose(jnt.second));
      tf_transform.header.stamp = time;
      tf_transform.header.frame_id = frame_prefix + seg->second.root;
      tf_transform.child_frame_id = frame_prefix + seg->second.tip;
      tf_transforms.push_back(tf_transform);
    }
  }
  tf_broadcaster_->sendTransform(tf_transforms);
}

要点：

  • 只使用 position，velocity / effort 被忽略
  • 每条 TF 是 单段 变换：parent_link → child_link
  • segment.pose(q) 用关节角 q 计算该段位姿（含 joint origin 的固定偏移）

示例 URDF（continuous 关节，origin xyz=”5 0 0” rpy=”0 0 1.57”）：

1
joint1 转 π 弧度 → link1→link2 的 TF translation.x ≈ 5.0

（集成测试 test_two_links_moving_joint.cpp 验证）

6.3 固定关节：publishFixedTransforms

1
2
3
4
5
6
7
8
9
10
void RobotStatePublisher::publishFixedTransforms()
{
  for (const auto & seg : segments_fixed_) {
    geometry_msgs::msg::TransformStamped tf_transform =
      kdlToTransform(seg.second.segment.pose(0));  // q=0
    tf_transform.header.stamp = now;
    ...
  }
  static_tf_broadcaster_->sendTransform(tf_transforms);
}

启动时发布一次；URDF 热更新后也会重新调用。

7. joint_states 回调逻辑

7.1 主流程

1
2
3
4
5
6
7
8
9
10
void RobotStatePublisher::callbackJointState(...)
{
  // 1. 校验 name.size == position.size
  // 2. 检测时间回退（bag 回放）→ 清空 last_publish_time_
  // 3. 节流判断
  // 4. 构建 joint_positions map
  // 5. 处理 mimic 关节
  // 6. publishTransforms
  // 7. 更新 last_publish_time_
}

7.2 Mimic 关节

1
2
3
4
5
6
7
for (const auto & i : mimic_) {
  if (joint_positions.find(i.second->joint_name) != joint_positions.end()) {
    double pos = joint_positions[i.second->joint_name] * i.second->multiplier +
      i.second->offset;
    joint_positions.insert(std::make_pair(i.first, pos));
  }
}

公式：q_mimic = q_source × multiplier + offset

mimic 关节名必须在 segments_ 中有对应段才会发布 TF；若 mimic 的是 fixed 关节，则该 mimic 关节本身也是 fixed，不会进入 segments_。

7.3 发布频率节流

1
2
3
4
5
6
7
rclcpp::Time current_time(state->header.stamp);
double publish_freq = this->get_parameter("publish_frequency").get_value<double>();
std::chrono::milliseconds publish_interval_ms =
  std::chrono::milliseconds(static_cast<uint64_t>(1000.0 / publish_freq));
rclcpp::Time max_publish_time = last_published + rclcpp::Duration(publish_interval_ms);
if (get_parameter("ignore_timestamp").get_value<bool>() ||
    current_time.nanoseconds() >= max_publish_time.nanoseconds())

逻辑：

  • 取所有关节中 最早 的 last_publish_time 作为 last_published
  • 仅当消息 header.stamp >= last_published + 1/freq 时才发布
  • 这是基于 消息时间戳 的节流，不是 wall-clock 定时器
  • ignore_timestamp=true 时每条消息都发布

7.4 时间回退处理

bag 回放若时间戳倒退，会清空 last_publish_time_ 并警告，避免 TF 被错误节流。

8. 动态 URDF 更新

两套机制配合：

8.1 参数校验（同步）

1
2
3
4
5
6
7
8
rcl_interfaces::msg::SetParametersResult RobotStatePublisher::parameterUpdate(...)
{
  if (parameter.get_name() == "robot_description") {
    if (new_urdf.empty()) { result.successful = false; ... }
    try { parseURDF(new_urdf, dummy_model); } catch (...) { result.successful = false; }
  }
  ...
}

8.2 参数事件（异步应用）

1
2
3
4
5
6
7
void RobotStatePublisher::onParameterEvent(...)
{
  if (event->node != this->get_fully_qualified_name()) return;
  // 过滤 robot_description CHANGED
  setupURDF(it.second->value.string_value);
  publishFixedTransforms();
}

更新后：

  1. 重建 segments_ / segments_fixed_ / mimic_
  2. 重新发布 /robot_description
  3. 重新发布 /tf_static

可动 TF 需等待新的 joint_states 才会更新。测试 test_two_links_change_fixed_joint.cpp 验证 fixed 关节从 xyz=5 改为 xyz=10 后 /tf_static 变化。

v3.0.3 修复了带 mimic 关节 URDF 重载时的崩溃（显式拷贝 JointMimic）。

9. 组件化部署

1
2
3
rclcpp_components_register_node(${PROJECT_NAME}_node
  PLUGIN "robot_state_publisher::RobotStatePublisher"
  EXECUTABLE robot_state_publisher)

两种运行方式：

1
2
3
4
5
# 独立可执行文件
ros2 run robot_state_publisher robot_state_publisher

# 组件容器内加载
ros2 component standalone robot_state_publisher robot_state_publisher::RobotStatePublisher

10. Launch 示例模式

launch/rsp-launch-urdf-file1.py 典型写法：

1
2
3
4
5
6
with open(urdf_file, 'r') as infp:
    robot_desc = infp.read()
params = {'robot_description': robot_desc}
Node(package='robot_state_publisher',
     executable='robot_state_publisher',
     parameters=[params])

其他 launch 示例：

  • 内联 URDF 字符串
  • xacro 命令行 / API / Command substitution
  • XML launch 格式

xacro 处理在 launch 层 完成，节点只接收最终 URDF 字符串。

11. 依赖关系

1
2
3
4
5
6
7
8
9
10
robot_state_publisher
  ├── urdf              # URDF 解析 (urdf::Model)
  ├── kdl_parser        # URDF → KDL::Tree
  ├── orocos_kdl        # KDL::Segment::pose()
  ├── tf2_ros           # TransformBroadcaster / StaticTransformBroadcaster
  ├── rclcpp            # Node、参数、订阅
  ├── rclcpp_components # 组件注册
  ├── sensor_msgs       # JointState
  ├── geometry_msgs     # TransformStamped
  └── std_msgs          # robot_description

12. 测试覆盖

测试 验证点
test_two_links_fixed_joint 固定关节 → /tf_static
test_two_links_fixed_joint_prefix frame_prefix 前缀
test_two_links_moving_joint continuous 关节 + joint_states → /tf
test_two_links_change_fixed_joint 运行时更新 URDF，fixed TF 变化
test_change_mimic_joint mimic 关节 URDF 热更新

13. 设计特点与局限

特点 说明
逐关节 TF 每条边独立发布，不做整树 FK 连乘
仅 1-DOF pose(double) 只支持单自由度关节
position only 忽略 velocity/effort
floating 不支持 6-DOF 浮动基座需其他节点发布
轻量 单文件 ~430 行，逻辑清晰
URDF 即参数 支持运行时热更新
局限 说明
无多 DOF 关节 planar / floating 等需特殊处理
节流基于消息 stamp 与 wall time 无关，bag 回放需 ignore_timestamp 或理解 stamp 逻辑
static TF 更新 URDF 变更后旧 /tf_static 不会自动“删除”旧 frame
mimic 仅处理 position 不传播 velocity

14. 典型数据流

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
launch 读取 robot.urdf / xacro
  ↓ 设置 robot_description 参数
robot_state_publisher 启动
  ↓ urdf::Model + kdl_parser → KDL::Tree
  ↓ addChildren → segments_ / segments_fixed_
  ↓ publishFixedTransforms() → /tf_static
  ↓ publish robot_description (transient_local)

joint_state_publisher / controller
  ↓ /joint_states {name, position}
callbackJointState
  ↓ mimic 计算
  ↓ segment.pose(q) → TransformStamped
  ↓ /tf

rviz / MoveIt / nav2
  ↓ tf2 Buffer 查询 link 位姿

15. 推荐阅读顺序

  1. README.md — 话题、参数、关节分类
  2. 构造函数 — 初始化顺序
  3. setupURDF + addChildren — URDF 如何变成 segment map
  4. callbackJointState + publishTransforms — 核心 FK→TF 链路
  5. kdl_parser 分析 — URDF 到 KDL 的转换细节
  6. 测试 — two_links_moving_joint.urdf + test_two_links_moving_joint.cpp
  7. launch 示例 — 如何在 bringup 中传入 URDF

16. 小结

robot_state_publisher 是 ROS 2 运动学链路的 URDF → tf2 桥接节点：用 kdl_parser 建 KDL 树，按关节类型分流到 /tf 与 /tf_static，订阅 joint_states 驱动可动关节变换，并支持 mimic 关节与 URDF 热更新。本身不做碰撞检测、逆运动学或整树 FK，职责单一、边界清晰。

如果你希望，我可以把本文写入 ros2doc/ros/robot_state_publisher 源码详细分析.md，或继续分析 rviz 如何用 TF + robot_description 渲染 RobotModel。

文章互动

阅读 --

留言

0 条留言

正在加载留言…