python_cmake_module 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros2/python_cmake_module
子包数量：1

1. 定位

python_cmake_module 目录含 1 个 ROS 2 包，工作区路径见下。

2. 子包列表

3. 在 ROS 2 Humble 栈中的关系

见 ros2总览.md 分层图。

4. 推荐阅读顺序

1. 阅读各子包 package.xml 2. 入口源码 3. 下游依赖方

5. 小结

python_cmake_module 为单包仓库，提供 python_cmake_module 功能。

rcl_interfaces 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros2/rcl_interfaces
版本：1.2.2（各子包统一），构建类型 ament_cmake + rosidl，许可证 Apache 2.0。

rcl_interfaces 仓库不是运行时逻辑库，而是一组 ROS 2 接口定义包（.msg / .srv / .action）。源码经 rosidl_generate_interfaces 生成 C/C++/Python 等语言的 typesupport 代码，供 rcl、rclcpp、rclpy、rcl_action、component_manager 等上层库使用。可以把它理解为 ROS 2 客户端栈的**“协议契约层”**——定义参数、Action、Lifecycle、组件化、时钟等核心概念在线上如何表达。

1. 总体认识

1.1 仓库定位

1.2 在 ROS 2 栈中的位置

flowchart TB
  subgraph apps [应用 / CLI]
    NODE[rclcpp Node]
    ROS2CLI[ros2 param / component]
    GAZEBO[Gazebo / 仿真 clock]
  end
  subgraph clients [客户端库]
    RCLCPP[rclcpp]
    RCLPY[rclpy]
    RCL[rcl]
    RCLACT[rcl_action]
    RCLLC[rcl_lifecycle]
  end
  subgraph iface [rcl_interfaces 仓库]
    RCLI[rcl_interfaces]
    ACT[action_msgs]
    LIFE[lifecycle_msgs]
    COMP[composition_interfaces]
    BUILTIN[builtin_interfaces]
    ROSGRAPH[rosgraph_msgs]
    STATS[statistics_msgs]
  end
  subgraph gen [代码生成]
    ROSIDL[rosidl_generate_interfaces]
    TS[typesupport C/C++/Python]
  end

  NODE --> RCLCPP --> RCLI & ACT & LIFE
  ROS2CLI --> COMP & RCLI
  GAZEBO --> ROSGRAPH
  RCL --> RCLI
  RCLACT --> ACT
  iface --> ROSIDL --> TS
  TS --> clients

2. 子包一览

仓库含 8 个独立 ROS 包（同一 git 仓库，colcon 分别编译）：

目录结构：

1
2
3
4
5
6
7
8
9
10
11

rcl_interfaces/
├── README.md
├── LICENSE
├── builtin_interfaces/       # Time, Duration
├── rcl_interfaces/           # 参数 + Log ★ 核心
├── action_msgs/              # GoalInfo, GoalStatus, CancelGoal
├── lifecycle_msgs/           # State, Transition, ChangeState...
├── composition_interfaces/   # LoadNode, UnloadNode, ListNodes
├── rosgraph_msgs/            # Clock
├── statistics_msgs/          # MetricsMessage
└── test_msgs/                # 测试专用 msg/action

3. 构建机制

每个子包结构相同，以 rcl_interfaces 为例：

1
2
3
4
5
6
7

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/FloatingPointRange.msg"
  "msg/IntegerRange.msg"
  ...
  "srv/SetParameters.srv"
  DEPENDENCIES builtin_interfaces
)

构建流程：

1
2
3
4
5

.msg / .srv / .action
    → rosidl_adapter（转 IDL）
    → rosidl_generator_c / _cpp / _py ...
    → rosidl_typesupport_c / _cpp / _fastrtps / _introspection ...
    → 安装到 install/rcl_interfaces/{include,lib,share}

所有包均声明 <member_of_group>rosidl_interface_packages</member_of_group>，表示属于 ROS 接口包生态。

3.1 包间依赖

4. builtin_interfaces — 基础时间类型

路径：builtin_interfaces/msg/

ROS 2 全栈最底层的时间/msg 类型，对应 OMG IDL PSM 中的 Time 和 Duration。

4.1 Time.msg

1
2
3
4
5

# The seconds component, valid over all int32 values.
int32 sec

# The nanoseconds component, valid in the range [0, 1e9)
uint32 nanosec

• 可表示负时间（如 {sec: -2, nanosec: 300000000} = -1.7s）
• 用于：std_msgs/Header.stamp、action_msgs/GoalInfo.stamp、参数事件时间戳等

4.2 Duration.msg

结构与 Time 相同，语义为时间间隔而非时间点。

4.3 消费者

builtin_interfaces 是 ROS 2 中使用最广泛的接口包之一，几乎所有消息类型都间接依赖它。

5. rcl_interfaces — 参数与日志

路径：rcl_interfaces/
这是仓库的核心包，定义 ROS 2 参数服务协议和 /rosout 日志消息。

5.1 参数系统架构

每个节点在自身 namespace 下暴露标准服务（由 rclcpp/rclpy 自动创建）：

标准 Topic：

README 中提到的 has_parameters 服务在 Humble 版接口文件中已不存在，以实际 .srv 文件为准。

5.2 参数值 — 变体类型设计

ParameterValue.msg 采用 tagged union（标记联合体） 模式：

1
2
3
4
5
6
7
8
9
10
11

uint8 type

bool bool_value
int64 integer_value
float64 double_value
string string_value
byte[] byte_array_value
bool[] bool_array_value
int64[] integer_array_value
float64[] double_array_value
string[] string_array_value

ParameterType.msg 定义 type 枚举：

只有 type 对应的一个字段有效——这是 ROS 2 参数在 DDS 线上无 union 类型时的惯用设计。

5.3 参数描述符与约束

ParameterDescriptor.msg 携带元数据：

• description / additional_constraints：人类可读说明
• read_only：初始化后不可改
• dynamic_typing：允许运行时改类型
• floating_point_range / integer_range：取值范围与步长

FloatingPointRange.msg 示例：from_value、to_value、step 三者定义合法浮点参数空间。

5.4 参数事件

ParameterEvent.msg 在一次原子更新中，每个参数名只出现在三个列表之一：

1
2
3
4
5

builtin_interfaces/Time stamp
string node
Parameter[] new_parameters
Parameter[] changed_parameters
Parameter[] deleted_parameters

rclcpp 的 ParameterEventHandler 和 ros2 param listen 订阅此 topic 跟踪参数变化。

5.5 关键 Service 定义

GetParameters.srv：

1
2
3

string[] names          # 请求
---
ParameterValue[] values # 响应，顺序与 names 对应

SetParameters.srv：

1
2
3

Parameter[] parameters
---
SetParametersResult[] results   # 每项含 successful + reason

ListParameters.srv：

1
2
3
4

string[] prefixes
uint64 depth                    # 0 = DEPTH_RECURSIVE 无限递归
---
ListParametersResult result     # names[] + prefixes[]

SetParametersAtomically.srv：与 SetParameters 类似，但返回单个 SetParametersResult，任一失败则全部回滚。

5.6 Log.msg — /rosout

1
2
3
4
5
6
7
8
9
10
11
12
13

byte DEBUG=10
byte INFO=20
byte WARN=30
byte ERROR=40
byte FATAL=50

builtin_interfaces/Time stamp
uint8 level
string name
string msg
string file
string function
uint32 line

• 日志级别与 Python logging / rcutils/logging.h 对齐
• rcl/logging_rosout.c 将 RCUTILS 日志发布到 rosout topic，类型即此消息
• 支持 ros2 run rqt_console 等工具订阅

5.7 ROS 1 桥接

mapping_rules.yaml 定义 rosgraph_msgs/Log → rcl_interfaces/Log 的字段映射，供 ros1_bridge 使用：

1
2
3
4
5
6
7
8

ros1_package_name: 'rosgraph_msgs'
ros1_message_name: 'Log'
ros2_package_name: 'rcl_interfaces'
ros2_message_name: 'Log'
fields_1_to_2:
  header.stamp: 'stamp'
  level: 'level'
  ...

6. action_msgs — Action 通用类型

路径：action_msgs/
定义所有 Action 共享的消息与服务，具体 Action（如 Fibonacci.action）由各功能包自行定义。

6.1 GoalInfo.msg

1
2

unique_identifier_msgs/UUID goal_id
builtin_interfaces/Time stamp

• goal_id：128 位 UUID，全局唯一标识一个 goal
• stamp：goal 被 server 接受的时间

6.2 GoalStatus.msg — 状态机

rcl_action 的 goal 状态机与此 enum 一一对应。

6.3 GoalStatusArray.msg

Action server 在 ~/action_name/_action/status topic 上发布，包含所有活跃 goal 的状态列表。

6.4 CancelGoal.srv

支持四种取消策略（goal_id + timestamp 组合）：

返回码：ERROR_NONE、ERROR_REJECTED、ERROR_UNKNOWN_GOAL_ID、ERROR_GOAL_TERMINATED。

6.5 与 Action 协议的关系

用户定义的 My.action 在底层会拆成多个 topic/service，action_msgs 提供元协议部分：

1
2
3
4
5

~/my_action/_action/send_goal      (service)
~/my_action/_action/cancel_goal    (CancelGoal.srv)  ← action_msgs
~/my_action/_action/get_result     (service)
~/my_action/_action/feedback       (topic)
~/my_action/_action/status         (GoalStatusArray) ← action_msgs

7. lifecycle_msgs — 生命周期节点

路径：lifecycle_msgs/
定义 Managed Node Lifecycle 的标准接口。

7.1 状态机

State.msg 定义 primary states 与 transition states：

Primary States（稳定态）：

Transition States（中间态）：CONFIGURING(10)、CLEANINGUP(11)、ACTIVATING(13)、DEACTIVATING(14) 等。

7.2 Transition.msg

定义标准转移 ID（0–8 为公开转移，10–62 为内部/回调结果）：

7.3 服务

7.4 TransitionEvent.msg

Lifecycle 节点在 ~/transition_event topic 发布状态变化：

1
2
3
4

uint64 timestamp
Transition transition
State start_state
State goal_state

rclcpp_lifecycle::LifecycleNode 和 ros2 lifecycle CLI 依赖这些接口。

8. composition_interfaces — 组件化节点

路径：composition_interfaces/
供 component_manager 和 ros2 component 动态加载/卸载 composable node。

8.1 LoadNode.srv

1
2
3
4
5
6
7
8
9
10
11
12
13

string package_name
string plugin_name
string node_name
string node_namespace
uint8 log_level
string[] remap_rules
rcl_interfaces/Parameter[] parameters
rcl_interfaces/Parameter[] extra_arguments
---
bool success
string error_message
string full_node_name
uint64 unique_id

• plugin_name：如 rclcpp_components 注册的 TalkerComponent
• 可携带 remap 规则和初始参数（复用 rcl_interfaces/Parameter）
• 返回 unique_id 供后续 UnloadNode 使用

8.2 UnloadNode.srv / ListNodes.srv

8.3 消费者

• rclcpp_components::ComponentManager
• ros2 component load/unload/list
• launch_ros 的 ComposableNodeContainer

9. rosgraph_msgs — 计算图时钟

路径：rosgraph_msgs/msg/Clock.msg

1

builtin_interfaces/Time clock

• 仿真环境（Gazebo 等）在 /clock topic 发布此消息
• rclcpp::TimeSource 订阅 /clock 驱动 ROS Time（sim time）
• 与 wall time（系统时钟）相对，enable use_sim_time 时节点 timer 跟随仿真时间

关键消费者：rclcpp/src/rclcpp/time_source.cpp

10. statistics_msgs — Topic 统计

路径：statistics_msgs/

MetricsMessage 结构：

• measurement_source_name：节点/topic/进程名
• metrics_source：指标名（如 message_age、subscription_period）
• window_start / window_stop：统计窗口
• statistics[]：数据点列表

消费者：rclcpp topic statistics 功能（libstatistics_collector），可选开启。

11. test_msgs — 测试专用

路径：test_msgs/

不应在应用代码中依赖——仅供 rosidl、rmw、rcl 测试使用。

12. 主要消费者映射

12.1 rcl 层直接使用

rcl 包在 package.xml 中依赖 rcl_interfaces，直接使用生成的 C 类型：

1

#include "rcl_interfaces/msg/log.h"

/rosout publisher 创建时使用 rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__Log()。

参数服务的实现不在 rcl 层，而在 rclcpp/rclpy 的 node_interfaces 中，但 wire 格式即本仓库定义的 srv/msg。

13. 参数分组规则

参数名采用类文件路径的分组语义（来自 README）：

• 默认组：/
• 嵌套组：/my_group/sub_group/param_name
• 与节点 namespace 独立——参数名本身是全局字符串

ListParameters 的 prefixes + depth 控制递归深度，DEPTH_RECURSIVE=0 表示无限深度。

14. 设计特点小结

15. 与相邻包对比

16. 推荐阅读顺序

1. rcl_interfaces/README.md — 参数 topic/service 命名约定
2. ParameterValue.msg + ParameterType.msg — 理解参数类型系统
3. GetParameters.srv / SetParameters.srv — 参数 RPC 协议
4. action_msgs/GoalStatus.msg — Action 状态机
5. lifecycle_msgs/State.msg + Transition.msg — 生命周期设计
6. 消费者代码：
   • rclcpp/src/rclcpp/node_interfaces/parameters*.cpp — 参数服务实现
   • rcl/src/rcl/logging_rosout.c — Log 消息发布
   • rclcpp/src/rclcpp/time_source.cpp — Clock 订阅
7. ROS 2 Design Articles — 参数、Action、Lifecycle 设计原文

17. 接口清单速查

17.1 rcl_interfaces 全部接口

Messages (11)：Parameter, ParameterValue, ParameterType, ParameterDescriptor, ParameterEvent, ParameterEventDescriptors, SetParametersResult, ListParametersResult, FloatingPointRange, IntegerRange, Log

Services (6)：GetParameters, SetParameters, SetParametersAtomically, ListParameters, DescribeParameters, GetParameterTypes

17.2 其他包

文档基于 ROS 2 Humble 工作区中的 rcl_interfaces 1.2.2 源码（接口定义）分析生成。

rcl_logging 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros2/rcl_logging
子包数量：3

1. 定位

rcl_logging 目录含 3 个 ROS 2 包，工作区路径见下。

2. 子包列表

3. 在 ROS 2 Humble 栈中的关系

见 ros2总览.md 分层图。

4. 推荐阅读顺序

1. 阅读各子包 package.xml 2. 入口源码 3. 下游依赖方

5. 小结

rcl_logging 为含 3 个子包的源码树，是 ROS 2 Humble 发行版的一部分。

rclcpp 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros2/rclcpp
版本：16.0.19（Humble），子包 4 个，构建类型 ament_cmake，语言 C++17，许可证 Apache 2.0。

rclcpp（ROS Client Library for C++）是 ROS 2 C++ 客户端库，几乎所有 C++ 节点、rviz2、Nav2、MoveIt 2 等均构建于此。它在 rcl（C API）之上提供类型安全的 C++ 抽象，并引入 Executor、callback_group、intra-process、参数服务 等 ROS 2 特有机制。理解 rclcpp 是掌握 ROS 2 C++ 应用并发模型与通信路径的关键。

1. 总体认识

1.1 核心职责

1.2 在 ROS 2 栈中的位置

与 rcl 的分工：rcl 提供语言无关 C API 与 wait set；参数服务的实现主体在 rclcpp（ParameterService），rcl 只负责 CLI 参数/YAML 解析。Executor、callback_group、intra-process 均为 rclcpp 独有，不在 rcl 层。

2. 子包结构

1
2
3
4
5
6
7
8
9

rclcpp/
├── rclcpp/                      # 核心库 ★
│   ├── include/rclcpp/          # ~85 个顶层头文件 + node_interfaces/ 等
│   ├── include/rclcpp/node_interfaces/  # 11 组 interface
│   ├── src/rclcpp/              # 73 个 .cpp（~11.8K 行）
│   └── src/rclcpp/executors/    # 4 种 Executor 实现
├── rclcpp_action/               # Action C++ API
├── rclcpp_components/           # Composable Node + ComponentManager
└── rclcpp_lifecycle/            # LifecycleNode

2.1 源码规模（核心包 rclcpp/rclcpp）

3. 依赖关系

3.1 rclcpp/package.xml

1
2
3
4
5
6
7
8
9
10
11

rclcpp
├── rcl                       # C 客户端库
├── rcl_yaml_param_parser     # YAML 参数文件（经 rcl 间接使用）
├── rmw                       # QoS 类型、GID 等
├── rcutils / rcpputils       # 日志、ScopeExit、文件系统
├── rosidl_runtime_cpp        # 消息 C++ 运行时
├── rosidl_typesupport_cpp    # typesupport 查找
├── rcl_interfaces            # 参数 srv/msg
├── rosgraph_msgs             # Clock 消息
├── libstatistics_collector   # topic 统计
└── tracetools                # LTTng 追踪点

3.2 子包额外依赖

4. 核心设计：Node + node_interfaces

4.1 Node 是用户 API 入口

1
2

/// Node is the single point of entry for creating publishers and subscribers.
class Node : public std::enable_shared_from_this<Node>

用户通过 Node::create_publisher()、create_subscription() 等创建通信实体；内部不直接持有 rcl_node_t，而是通过 组合式 interface 拆分职责。

4.2 Interface 组合（构造顺序）

Node 构造函数按固定顺序实例化各 interface，并相互注入依赖：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

Node::Node(
  const std::string & node_name,
  const std::string & namespace_,
  const NodeOptions & options)
: node_base_(new rclcpp::node_interfaces::NodeBase(...)),
  node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
  node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())),
  node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
  node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())),
  node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
  node_clock_(new rclcpp::node_interfaces::NodeClock(...)),
  node_parameters_(new rclcpp::node_interfaces::NodeParameters(...)),
  node_time_source_(new rclcpp::node_interfaces::NodeTimeSource(...)),
  node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
  ...

设计动机：

• Component 友好：ComponentManager 只需 NodeBaseInterface 等指针即可把节点挂到 Executor
• 可测试：mock 单个 interface 而不构造完整 Node
• LifecycleNode 复用：rclcpp_lifecycle::LifecycleNode 同样组合这些 interface

4.3 NodeOptions

NodeOptions 集中配置：

• context()、use_intra_process_comms()、enable_topic_statistics()
• start_parameter_services()、parameter_overrides()、allow_undeclared_parameters()
• get_rcl_node_options() → 底层 rcl_node_options_t（含 remap、use_global_arguments）
• QoS 预设：parameter_event_qos()、clock_qos() 等

5. Context 与 init

5.1 全局 init 流程

1
2
3
4
5
6
7
8
9
10
11

void
init(
  int argc,
  char const * const * argv,
  const InitOptions & init_options,
  SignalHandlerOptions signal_handler_options)
{
  using rclcpp::contexts::get_global_default_context;
  get_global_default_context()->init(argc, argv, init_options);
  install_signal_handlers(signal_handler_options);
}

rclcpp::init() → 默认 Context::init() → rcl_init()，并安装 SIGINT/SIGTERM 处理器（触发 shutdown）。

5.2 Context 封装 rcl_context

1
2
3
4
5
6
7
8
9
10
11
12
13

Context::init(
  int argc,
  char const * const * argv,
  const rclcpp::InitOptions & init_options)
{
  ...
  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), context);
  ...
  rcl_context_.reset(context, __delete_context);
  if (init_options.auto_initialize_logging()) {
    rcl_logging_configure_with_output_handler(...);
  }
}

5.3 辅助 API

6. Executor 架构

Executor 是 rclcpp 并发与调度核心：将「通信图」与「执行模型」解耦——节点创建实体，Executor 决定何时执行回调。

6.1 类层次

6.2 spin 主循环（SingleThreadedExecutor）

1
2
3
4
5
6
7
8
9
10
11
12
13
14

void
SingleThreadedExecutor::spin()
{
  if (spinning.exchange(true)) {
    throw std::runtime_error("spin() called while already spinning");
  }
  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
  while (rclcpp::ok(this->context_) && spinning.load()) {
    rclcpp::AnyExecutable any_executable;
    if (get_next_executable(any_executable)) {
      execute_any_executable(any_executable);
    }
  }
}

6.3 get_next_executable 两阶段

1
2
3
4
5
6
7
8
9
10
11
12
13

bool
Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout)
{
  bool success = get_next_ready_executable(any_executable);
  if (!success) {
    wait_for_work(timeout);
    if (!spinning.load()) {
      return false;
    }
    success = get_next_ready_executable(any_executable);
  }
  return success;
}

1. get_next_ready_executable：按优先级扫描已 ready 实体（timer → subscription → service → client → waitable）
2. wait_for_work：若无 ready 实体，则 collect_entities → rcl_wait_set_resize → rcl_wait()

6.4 wait_for_work 与 rcl wait set

核心步骤（executor.cpp）：

1. memory_strategy_->collect_entities(weak_groups_to_nodes_) — 收集本 Executor 管理的 callback group 内实体
2. rcl_wait_set_clear / rcl_wait_set_resize — 按实体数量调整 wait set
3. memory_strategy_->add_handles_to_wait_set — 填入 subscription/timer/service/client/guard_condition 句柄
4. rcl_wait(&wait_set_, timeout) — 阻塞直至有事件或超时
5. remove_null_handles — 清理 middleware 标记为 invalid 的句柄

这与 rcl 源码分析 中的 wait set 机制直接对应，rclcpp 在其上增加了 callback group 过滤 与 memory strategy 抽象。

6.5 execute_any_executable

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
Executor::execute_any_executable(AnyExecutable & any_exec)
{
  if (!spinning.load()) {
    return;
  }
  if (any_exec.timer) {
    execute_timer(any_exec.timer);
  }
  if (any_exec.subscription) {
    execute_subscription(any_exec.subscription);
  }
  if (any_exec.service) {
    execute_service(any_exec.service);
  }
  if (any_exec.client) {
    execute_client(any_exec.client);
  }
  if (any_exec.waitable) {
    any_exec.waitable->execute(any_exec.data);
  }
  any_exec.callback_group->can_be_taken_from().store(true);
  interrupt_guard_condition_.trigger();
}

执行完毕后：

• 重置 can_be_taken_from（MutuallyExclusive group 在执行前会被置 false）
• 触发 interrupt_guard_condition_，唤醒可能在 rcl_wait 中阻塞的其他线程

6.6 MultiThreadedExecutor

• 默认线程数 = hardware_concurrency()（至少 1）
• 各线程在 wait_mutex_ 保护下调用 get_next_executable
• 可选 yield_before_execute 减少锁竞争
• any_exec.callback_group.reset() 避免析构时错误重置 group 状态

6.7 StaticSingleThreadedExecutor

实体列表在 spin() 前通过 StaticExecutorEntitiesCollector 一次性收集，仅在 add/remove node 时更新。适合实体集合固定的生产节点，降低 collect_entities 开销。

6.8 MemoryStrategy

默认 AllocatorMemoryStrategy（memory_strategies/allocator_memory_strategy.hpp）负责：

• 从 callback group → node 映射中收集 handles
• 在 wait 返回后查找 next ready 实体
• 可替换为自定义 strategy（测试或特殊调度）

7. CallbackGroup 并发模型

7.1 两种类型

1
2
3
4
5

enum class CallbackGroupType
{
  MutuallyExclusive,
  Reentrant
};

7.2 与 Executor 的关系

• 每个 subscription/timer/service/client 创建时绑定一个 CallbackGroup
• 默认 group 由 automatically_add_to_executor_with_node 控制是否随 add_node() 自动注册
• Executor 维护 weak_groups_to_nodes_ 映射，只调度已 add_callback_group() 的 group
• MutuallyExclusive：get_next_ready_executable_from_map 选中实体后将 can_be_taken_from 置 false，直到 execute 完成

7.3 典型用法

1
2
3
4
5
6
7

// 互斥：回调不会重叠
auto group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions opts;
opts.callback_group = group;

// 可重入：多线程 Executor 下可并行
auto reentrant = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant);

8. Publisher / Subscription

8.1 模板层次

还支持 TypeAdapter（自定义类型适配 ROS 消息）、GenericPublisher/Subscription（运行时类型）。

8.2 发布路径（inter-process）

1
2
3
4
5

Publisher::publish(msg)
  → do_inter_process_publish()
    → rcl_publish()
      → rmw_publish()
        → DDS

8.3 发布路径（intra-process 开启）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

publish(std::unique_ptr<T, ROSMessageTypeDeleter> msg)
{
  if (!intra_process_is_enabled_) {
    this->do_inter_process_publish(*msg);
    return;
  }
  bool inter_process_publish_needed =
    get_subscription_count() > get_intra_process_subscription_count();

  if (inter_process_publish_needed) {
    auto shared_msg =
      this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg));
    this->do_inter_process_publish(*shared_msg);
  } else {
    this->do_intra_process_ros_message_publish(std::move(msg));
  }
}

策略要点：

• 仅 Volatile durability 允许 intra-process
• 若存在跨进程订阅者，先 intra 再 inter（降低端到端延迟）
• unique_ptr 路径优先零拷贝移交所有权

8.4 订阅执行路径（Executor）

execute_subscription() 三条分支（executor.cpp）：

Intra-process 消息在 handle_message 内通过 MessageInfo.from_intra_process 区分，不经过 DDS take。

9. Intra-process 通信

9.1 IntraProcessManager

位于 rclcpp/experimental/intra_process_manager.hpp，由 Context 持有单例（每 context 一个）。

1
2
3
4
5
6
7
8
9
10
11
12
13
14

uint64_t
IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher)
{
  uint64_t pub_id = IntraProcessManager::get_next_unique_id();
  publishers_[pub_id] = publisher;
  pub_to_subs_[pub_id] = SplittedSubscriptions();
  for (auto & pair : subscriptions_) {
    ...
    if (can_communicate(publisher, subscription)) {
      insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method());
    }
  }
  return pub_id;
}

9.2 匹配规则 can_communicate

1
2
3
4
5
6
7
8
9
10
11
12

bool
IntraProcessManager::can_communicate(...) const
{
  if (strcmp(pub->get_topic_name(), sub->get_topic_name()) != 0) {
    return false;
  }
  auto check_result = rclcpp::qos_check_compatible(pub->get_actual_qos(), sub->get_actual_qos());
  if (check_result.compatibility == rclcpp::QoSCompatibility::Error) {
    return false;
  }
  return true;
}

9.3 数据结构

两种 intra subscription 策略：共享指针（多订阅者共享同一份数据）与 所有权转移（unique_ptr 零拷贝）。

10. Service / Client / Timer / Waitable

Timer 基于 rcl timer + guard condition：到期时在 wait set 中触发，优先级高于 subscription（get_next_ready_executable_from_map 先查 timer）。

Waitable 是可扩展钩子：rclcpp_action::Client 实现 Waitable 接口，将 action 相关 waitable 实体纳入 Executor 调度，而无需独立 spin 线程。

11. 参数系统

11.1 NodeParameters

NodeParameters 负责：

• declare_parameter() / get_parameter() / set_parameter()
• 参数覆盖（CLI、--params-file、节点级 overrides）
• on_parameters_set 回调链
• 可选启动 ParameterService 与 /parameter_events 发布者

11.2 ParameterService（标准服务）

1
2
3
4
5
6
7
8
9
10

get_parameters_service_ = create_service<rcl_interfaces::srv::GetParameters>(
  node_base, node_services,
  node_name + "/" + parameter_service_names::get_parameters,
  [node_params](..., Request request, Response response)
  {
    auto parameters = node_params->get_parameters(request->names);
    for (const auto & param : parameters) {
      response->values.push_back(param.get_value_message());
    }
  }, ...);

注册的服务（与 rcl_interfaces 分析 对应）：

11.3 ParameterClient

远程节点参数访问的客户端封装，内部创建对应 service client，供 ros2 param 等工具链使用。

12. 时间与 Clock

12.1 TimeSource

TimeSource（time_source.cpp）订阅全局 /clock（rosgraph_msgs/msg/Clock）：

• 收到 sim time 时启用 ros_time_active_，更新所有关联 Clock
• 与 use_sim_time 参数联动（经 NodeTimeSource wiring）
• 支持独立 clock 线程（use_clock_thread NodeOption）

12.2 Clock 类型

Timer 可选择 clock：create_timer(clock, period, callback) vs create_wall_timer()。

13. QoS

rclcpp::QoS 封装 rmw_qos_profile_t，提供流式 API：

1
2
3

rclcpp::QoS(10).reliable().transient_local();
rclcpp::SensorDataQoS();
rclcpp::ParametersQoS();

13.1 qos_overrides

节点构造时通过 declare_qos_parameters() 暴露 qos_overrides.<topic>.<policy> 参数，允许运行时覆盖 depth/reliability/durability/history（见 node.cpp 中 get_parameter_events_qos）。

13.2 兼容性检查

qos_check_compatible() 在 intra-process 匹配与 rclcpp::QoS 警告中使用，对应 RMW 的 rmw_qos_profile_check_compatible。

14. rclcpp_action

依赖 rcl_action C 库，提供类型安全的 C++ Action API。

14.1 主要类型

14.2 与 Executor 集成

Action client/server 内部创建多个 pub/sub/service 及 Waitable 实体，注册到 callback group 后由 Executor 统一调度，无需用户手动 spin action 专用线程。

14.3 依赖链

1
2
3

rclcpp_action → rclcpp → rcl
              → rcl_action → rcl
              → action_msgs（UUID 等）

15. rclcpp_components

15.1 动机

Composable Node 允许在 单进程 内加载多个节点组件，配合 intra-process 减少序列化与 DDS hop。

15.2 ComponentManager

1
2
3
4
5
6
7
8

ComponentManager::ComponentManager(...)
: Node(std::move(node_name), node_options),
  executor_(executor)
{
  loadNode_srv_ = create_service<LoadNode>("~/_container/load_node", ...);
  unloadNode_srv_ = create_service<UnloadNode>("~/_container/unload_node", ...);
  listNodes_srv_ = create_service<ListNodes>("~/_container/list_nodes", ...);
}

15.3 加载流程

1. ament_index 查找包内注册的组件资源（rclcpp_components_register_nodes CMake 宏）
2. class_loader::ClassLoader 动态加载 .so
3. NodeFactory 实例化组件（RCLCPP_COMPONENTS_REGISTER_NODE 宏）
4. executor->add_node() 将新节点纳入调度

15.4 容器可执行文件

16. rclcpp_lifecycle

16.1 LifecycleNode

继承/组合与 Node 相同的 interface，额外实现 LifecycleNodeInterface：

转移：configure → activate → deactivate → cleanup → shutdown 等，底层调用 rcl_lifecycle。

16.2 LifecyclePublisher

仅在 Active 状态下真正 publish；Inactive 时 publish 被忽略或缓存（取决于配置），便于安全切换。

16.3 与 Nav2 / 工业场景

生命周期节点是托管节点（managed node）模式的基础，配合 lifecycle_manager 统一拉起/关闭。

17. 关键数据路径

17.1 订阅回调端到端

17.2 发布端到端（含 intra-process）

17.3 进程启动典型顺序

1
2
3
4
5
6
7
8
9
10
11

main()
  → rclcpp::init(argc, argv)
    → Context::init → rcl_init
  → auto node = std::make_shared<Node>(...)
    → NodeBase → rcl_node_init
    → NodeParameters → 声明参数 / 启动 ParameterService
    → NodeTimeSource → 订阅 /clock
  → rclcpp::spin(node)
    → SingleThreadedExecutor::add_node
    → spin loop (wait + execute)
  → rclcpp::shutdown()

18. 目录与模块索引

18.1 include/rclcpp/ 主要头文件

18.2 src/rclcpp/ 核心实现

19. 与 rcl / rcl_interfaces 对照

20. 调试与追踪

• 日志：RCLCPP_* 宏 → rcutils logging；节点 logger 名来自 NodeLogging
• 追踪：TRACEPOINT(rclcpp_executor_*) 等，依赖 tracetools
• Topic 统计：libstatistics_collector 可选编译，经 subscription 回调统计
• 常见问题：
  • 回调不触发 → 检查 Executor 是否 add_node、callback group 是否注册
  • 死锁 → MutuallyExclusive group 内回调再次 spin 或阻塞同 group 实体
  • intra-process 不生效 → QoS durability 非 Volatile、topic 名/QoS 不匹配

21. 推荐阅读顺序

1. utilities.cpp + context.cpp — 理解 init/shutdown 与默认 context
2. node.cpp + node_interfaces/node_topics.cpp — Node 如何创建 pub/sub
3. executors/single_threaded_executor.cpp + executor.cpp — spin / wait / execute 全流程
4. callback_group.hpp + executor.cpp（get_next_ready_executable_from_map） — 并发语义
5. publisher.hpp + intra_process_manager.cpp — 发布与进程内优化
6. parameter_service.cpp + node_interfaces/node_parameters.cpp — 参数栈
7. time_source.cpp — sim time
8. rclcpp_action/client.hpp — action 如何挂到 Executor
9. rclcpp_components/component_manager.cpp — 动态组件
10. rclcpp_lifecycle/lifecycle_node.hpp — 生命周期
11. 对照 examples/rclcpp_* 与 rcl 源码分析 下层行为

22. 小结

rclcpp 是 ROS 2 C++ 应用的主 API 层，核心模式为：

• Node + node_interfaces 创建与管理通信实体
• Executor 通过 rcl_wait 驱动 callback_group 内的回调
• IntraProcessManager 在同进程内绕过 DDS 实现低延迟数据路径
• ParameterService / TimeSource 等将 ROS 2 系统服务集成进节点生命周期

掌握 Executor + callback_group 的交互，是理解 ROS 2 C++ 并发模型的关键；排查通信问题时，应沿 publish/take → rcl → rmw 链向下追踪，并区分 inter-process 与 intra-process 路径。

rclpy 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros2/rclpy
版本：3.3.21（Humble），单包仓库，构建类型 ament_cmake（Python + C++ 扩展），许可证 Apache 2.0。

rclpy 是 ROS 2 Python 客户端库，为 ros2cli、Launch 测试节点、大量 Python 应用提供主 API。它在 rcl（C API）之上通过 pybind11 扩展 _rclpy_pybind11 封装底层句柄，在纯 Python 层实现 Executor、callback_group、参数服务、async 回调 等机制。与 rclcpp 功能对齐，但架构为「Python 调度 + C 绑定」双层设计。

1. 总体认识

1.1 核心职责

1.2 在 ROS 2 栈中的位置

与 rcl 的分工：与 rclcpp 相同，参数服务实现主体在 rclpy；rcl 负责 CLI/YAML 参数解析。Executor、callback_group 为 rclpy 独有 Python 层逻辑。

2. 仓库结构

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

rclpy/
├── rclpy/                          # 主包 ★
│   ├── rclpy/                      # Python 源码（~50 个 .py）
│   │   ├── node.py                 # 最大（~1957 行）
│   │   ├── executors.py            # Executor（~870 行）
│   │   ├── qos.py                  # QoS（~499 行）
│   │   ├── action/                 # Action client/server
│   │   ├── lifecycle/              # LifecycleNode
│   │   └── impl/                   # C 扩展懒加载
│   ├── src/rclpy/                  # C++ 绑定（30 个 .cpp）
│   │   ├── _rclpy_pybind11.cpp     # 模块入口
│   │   ├── wait_set.cpp            # rcl_wait_set 封装
│   │   ├── node.cpp / publisher.cpp / subscription.cpp ...
│   │   └── action_*.cpp / lifecycle.cpp
│   ├── test/                       # pytest + gtest
│   └── CMakeLists.txt              # 构建 Python 包 + 扩展
└── (无独立子 package.xml)

2.1 源码规模

3. 依赖关系（package.xml）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

rclpy
├── rcl                       # C 客户端库
├── rcl_action                # Action C API（扩展层直接调用）
├── rcl_lifecycle             # Lifecycle C API
├── rcl_yaml_param_parser     # YAML 参数（经 Node C++ 层）
├── rcl_logging_interface     # 日志接口
├── rmw / rmw_implementation # 中间件
├── rcutils / rcpputils       # 工具库
├── rosidl_runtime_c          # 消息 C 运行时（扩展层序列化）
├── pybind11_vendor           # Python 绑定
├── rcl_interfaces            # 参数 srv/msg（exec_depend）
├── rosgraph_msgs             # /clock
├── builtin_interfaces        # Time 等
├── unique_identifier_msgs    # Action goal UUID
└── rpyutils                  # import_c_library 工具

4. 双层架构：Python ↔ _rclpy_pybind11

4.1 懒加载 C 扩展

为避免 import rclpy 时立即加载 C 库，扩展通过单例延迟导入：

1
2
3
4

from rpyutils import import_c_library
package = 'rclpy'

rclpy_implementation = import_c_library('._rclpy_pybind11', package)

各模块在需要时 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy。

4.2 pybind11 模块导出

_rclpy_pybind11.cpp 注册所有 C 层类型与自由函数：

4.3 Destroyable：句柄生命周期

C++ 层所有 rcl 句柄继承 Destroyable，实现 Python 上下文管理器：

1
2
3
4
5
6
7
8
9
10
11

class Destroyable
{
public:
  void enter();    // __enter__ — 阻止销毁
  void exit(...);  // __exit__ — 允许销毁
  void destroy_when_not_in_use();
  virtual void destroy() = 0;
private:
  size_t use_count = 0u;
  bool please_destroy_ = false;
};

Python 侧统一使用 with handle: 保护 rcl 调用，避免 wait 期间句柄被 GC 销毁：

1
2

with self.handle:
    self.__publisher.publish(msg)

5. Context 与 init

5.1 Python Context

1
2
3
4
5
6
7
8
9

class Context:
    def init(self, args=None, *, initialize_logging=True, domain_id=None):
        from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
        ...
        self.__context = _rclpy.Context(
            args if args is not None else sys.argv,
            domain_id if domain_id is not None else _rclpy.RCL_DEFAULT_DOMAIN_ID)
        if initialize_logging and not self._logging_initialized:
            _rclpy.rclpy_logging_configure(self.__context)

5.2 C++ Context → rcl_init

1
2
3
4
5
6

Context::Context(py::list pyargs, size_t domain_id)
{
  rcl_context_ = std::shared_ptr<rcl_context_t>(...);
  *rcl_context_ = rcl_get_zero_initialized_context();
  // rcl_init_options + rcl_init ...
}

全局 g_contexts 向量跟踪所有有效 context，进程退出时 shutdown_contexts() 统一 shutdown。

5.3 便捷 API（rclpy/__init__.py + utilities.py）

6. Node

6.1 实体容器

Node 在 Python 层维护所有通信实体的列表（rclcpp 则分散在 node_interfaces 中）：

1
2
3
4
5
6
7
8

self._publishers: List[Publisher] = []
self._subscriptions: List[Subscription] = []
self._clients: List[Client] = []
self._services: List[Service] = []
self._timers: List[Timer] = []
self._guards: List[GuardCondition] = []
self.__waitables: List[Waitable] = []
self._default_callback_group = MutuallyExclusiveCallbackGroup()

底层 rcl_node_t 由 C++ rclpy::Node 持有；Python Node 通过 property 暴露 handle。

6.2 构造流程

1. 校验 context 已 init
2. _rclpy.Node(name, namespace, context, cli_args, use_global_arguments, enable_rosout)
3. 创建 logger、TimeSource、ParameterService（可选）
4. 处理 parameter_overrides、qos_overrides 声明

6.3 create_* 方法

创建时实体加入指定 callback_group（默认 MutuallyExclusiveCallbackGroup），并 append 到 Node 对应列表。

7. Publisher / Subscription

7.1 Publisher

Python 薄包装，核心 publish 在 C++：

1
2
3
4
5
6

def publish(self, msg: Union[MsgType, bytes]) -> None:
    with self.handle:
        if isinstance(msg, self.msg_type):
            self.__publisher.publish(msg)
        elif isinstance(msg, bytes):
            self.__publisher.publish_raw(msg)

C++ 层将 Python 消息对象转为 C 结构后调用 rcl_publish：

1
2
3
4
5
6

void Publisher::publish(py::object pymsg)
{
  auto raw_ros_message = convert_from_py(pymsg);
  rcl_ret_t ret = rcl_publish(rcl_publisher_.get(), raw_ros_message.get(), NULL);
  ...
}

消息转换依赖 rosidl_generator_py 生成的 C 绑定与 common_get_type_support() 查找 typesupport。

7.2 Subscription

• take_message() 在 C++ 层完成（rcl_take + Python 对象构造）
• 支持 raw=True 返回 bytes
• _executor_event 标志防止同一 subscription 被重复加入 wait set

7.3 数据路径

1
2

publish:  Python msg → convert_from_py → rcl_publish → rmw → DDS
take:     DDS → rmw → rcl_take → Python msg → user callback

注意：Humble 版 rclpy 无 intra-process 优化，所有消息均走完整 rcl/rmw 路径。同进程 Python 节点间通信仍经 DDS（与 rclcpp intra-process 不同）。

8. Executor 架构

Executor 是 rclpy 最复杂的纯 Python 模块（870 行），负责 wait → take → execute 全流程。

8.1 类层次

8.2 spin 流程

1
2
3
4

# SingleThreadedExecutor
while rclpy.ok() and not self._is_shutdown:
    handler, entity, node = wait_for_ready_callbacks(timeout)
    handler()  # 同步执行 Task

1
2
3

# MultiThreadedExecutor
handler, entity, node = wait_for_ready_callbacks(...)
self._executor.submit(handler)  # 线程池异步执行

8.3 _wait_for_ready_callbacks 核心逻辑

关键步骤（executors.py）：

1. 遍历节点，收集 can_execute 过滤后的 subscriptions/timers/clients/services/guards/waitables
2. 用 _rclpy.WaitSet 封装 rcl_wait_set_init/clear/add/wait
3. wait_set.wait(timeout_nsec) 阻塞
4. 按 ready 索引匹配实体，调用 _make_handler 生成 Task
5. 通过 generator yield 返回 (handler, entity, node)

8.4 Task 与 async 回调

rclpy 独有：回调可以是 coroutine。

1
2
3
4
5

async def await_or_execute(callback, *args):
    if inspect.iscoroutinefunction(callback):
        return await callback(*args)
    else:
        return callback(*args)

_make_handler 创建 async Task，流程：

1. callback_group.beginning_execution(entity) — MutuallyExclusive 加锁
2. take_from_wait_list(entity) — take 消息/请求
3. await call_coroutine(entity, arg) — 执行用户回调
4. callback_group.ending_execution(entity) — 释放锁
5. 触发 guard condition 唤醒 wait

8.5 WaitSet C++ 封装

1
2
3
4
5
6
7

WaitSet::WaitSet(..., Context & context)
{
  rcl_wait_set_init(..., context.rcl_ptr(), ...);
}
// add_subscription / add_timer / add_service / add_client / add_guard_condition
// wait() → rcl_wait()
// get_ready_entities() → 返回 ready 句柄 pointer 集合

与 rclcpp Executor::wait_for_work 直接对应，但 rclpy 在 Python 层 构建 wait set（每轮循环重建），而非 rclcpp 的 MemoryStrategy。

9. CallbackGroup 并发模型

1
2

# ReentrantCallbackGroup — can_execute 恒 True
# MutuallyExclusiveCallbackGroup — _active_entity 锁，同时仅一个回调

与 rclcpp 的 can_be_taken_from 原子标志语义等价，但 rclpy 用 Python Lock 实现。

Executor.can_execute 额外检查 entity._executor_event：已生成 handler 但未执行完毕的实体不再进入 wait set。

10. 参数系统

10.1 Node 内参数存储

• _parameters: dict — 参数名 → Parameter
• _descriptors — 参数描述符
• _parameters_callbacks — on_parameters_set 链
• 支持 allow_undeclared_parameters、automatically_declare_parameters_from_overrides

10.2 ParameterService

1
2
3
4
5
6
7
8

class ParameterService:
    def __init__(self, node):
        node.create_service(DescribeParameters, nodename + '/describe_parameters', ...)
        node.create_service(GetParameters, nodename + '/get_parameters', ...)
        node.create_service(GetParameterTypes, ...)
        node.create_service(ListParameters, ...)
        node.create_service(SetParameters, ...)
        node.create_service(SetParametersAtomically, ...)

服务名格式为 <node_name>/<service_suffix>（与 rclcpp 的 FQN 路径等价，表达方式不同）。类型均来自 rcl_interfaces（参见 rcl_interfaces 分析）。

10.3 TimeSource 与 use_sim_time

TimeSource 订阅 /clock，监听 use_sim_time 参数变化，更新关联 ROSClock：

1
2
3
4
5

def _subscribe_to_clock_topic(self):
    self._clock_sub = node.create_subscription(
        rosgraph_msgs.msg.Clock, CLOCK_TOPIC,
        self.clock_callback,
        QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT))

11. QoS

qos.py（499 行）提供：

• QoSProfile 类（depth、reliability、durability、history、liveliness 等）
• 预设：qos_profile_sensor_data、qos_profile_parameters、qos_profile_services_default 等
• qos_overriding_options.py — 与 rclcpp 对齐的运行时 QoS 覆盖

C 层 rclpy_qos_check_compatible() 暴露 RMW 兼容性检查。

12. Action（rclpy.action）

同仓库子模块，非独立 package。

C 绑定：action_client.cpp（322 行）、action_server.cpp（430 行），直接调用 rcl_action_*。

Action client 作为 Waitable 注册到 Executor，与 rclcpp action client 设计一致。

13. Lifecycle（rclpy.lifecycle）

C 绑定：lifecycle.cpp（364 行）封装 rcl_lifecycle 状态机。

状态转移回调：on_configure、on_activate、on_deactivate、on_cleanup、on_shutdown 等。

14. 其他重要模块

15. 关键数据路径

15.1 订阅回调端到端

15.2 典型程序结构

1
2
3
4
5
6
7
8
9
10
11

import rclpy
from rclpy.node import Node

def main():
    rclpy.init()
    node = Node('my_node')
    pub = node.create_publisher(String, 'topic', 10)
    sub = node.create_subscription(String, 'topic', callback, 10)
    rclpy.spin(node)   # 全局 SingleThreadedExecutor
    node.destroy_node()
    rclpy.shutdown()

等价于 rclcpp 的 init → Node → spin → shutdown，但 rclpy.spin 使用模块级全局 Executor（__init__.py 中 get_global_executor()）。

16. C++ 扩展文件索引

17. 与 rclcpp 对照

两者均调用相同 rcl_* API，调试通信问题时应沿 rcl → rmw 链向下追踪。

18. 调试与测试

• 日志：RCLCPP_* 对应 rclpy 的 node.get_logger().info() 等
• 异常：C 层错误转为 RCLError/RMWError/InvalidHandle
• 测试：test/ 下 ~40 个 pytest 文件 + C++ gtest（test_python_allocator.cpp）
• 常见问题：
  • NotInitializedException — 未调用 rclpy.init()
  • 回调不触发 — 未 spin 或实体不在 Executor 管理的节点中
  • InvalidHandle — 在 with handle 外使用已销毁句柄
  • async 回调在 SingleThreadedExecutor 中需 Task 驱动（不支持裸 event loop）

19. 推荐阅读顺序

1. impl/implementation_singleton.py + _rclpy_pybind11.cpp — 理解绑定入口
2. context.py + context.cpp — init/shutdown
3. node.py（构造 + create_publisher/subscription） — 用户 API
4. publisher.py + publisher.cpp — publish 数据路径
5. executors.py（_wait_for_ready_callbacks + _make_handler） — 调度核心
6. callback_groups.py — 并发语义
7. wait_set.cpp — 底层 wait
8. parameter_service.py + parameter.py — 参数栈
9. time_source.py — sim time
10. action/client.py + action_client.cpp — Action 与 Waitable
11. lifecycle/node.py — 生命周期
12. 对照 rcl 源码分析、rclcpp 源码分析

20. 小结

rclpy 是 ROS 2 Python 应用的主 API 层，采用 Python 调度 + pybind11 C 绑定 双层架构：

• Python 层（node.py、executors.py）负责 Entity 管理、Executor 调度、async 回调、参数服务
• C 扩展（_rclpy_pybind11）负责 rcl_* 调用、消息转换、WaitSet、Action/Lifecycle 绑定
• Destroyable + with handle 保证多线程 wait 期间句柄安全
• Task/async 是 rclpy 相对 rclcpp 的显著差异，支持 coroutine 风格节点

掌握 _wait_for_ready_callbacks → _make_handler → Task 执行 链路，是理解 rclpy 并发模型的关键。

rcl 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros2/rcl
版本：5.3.13（Humble），子包 4 个，构建类型 ament_cmake，语言 C，许可证 Apache 2.0。

rcl（ROS Client Library）是 ROS 2 客户端库的 C 实现层，封装 rmw（ROS Middleware Interface），为 rclcpp、rclpy 及其他语言绑定提供语言无关、稳定 ABI 边界的统一 C API。所有 DDS/中间件细节都在 rcl 以下；调试通信问题时常需追到 rcl → rmw 返回值链。

1. 总体认识

1.1 核心职责

1.2 在 ROS 2 栈中的位置

注意：参数服务（get_parameters 等）的实现主体在 rclcpp/rclpy，不在 rcl 核心库内。rcl 只负责解析 --params-file YAML（通过 rcl_yaml_param_parser），供上层加载初始参数。

2. 子包结构

1
2
3
4
5
6
7

rcl/
├── rcl/                      # 核心库 ★
│   ├── include/rcl/          # 37 个公开头文件
│   └── src/rcl/              # 32 个 .c 实现（~11.5K 行）
├── rcl_action/               # Action C API
├── rcl_lifecycle/            # Lifecycle 状态机
└── rcl_yaml_param_parser/    # YAML 参数文件解析

2.1 依赖关系（rcl/package.xml）

1
2
3
4
5
6
7
8

rcl
├── rmw_implementation    # 运行时加载的 RMW 实现
├── rcutils               # 日志、错误、分配器、时间
├── rosidl_runtime_c      # 消息 typesupport 接口
├── rcl_interfaces        # 内置服务/msg 定义
├── rcl_yaml_param_parser # YAML 参数解析
├── rcl_logging_spdlog    # 默认日志后端
└── tracetools            # LTTng 追踪点

3. 模块划分（官方文档）

rcl.h 是总入口，按 ROS 概念组织 API：

1
2
3
4
5
6
7
8
9

* - Nodes          → rcl/node.h
* - Publisher      → rcl/publisher.h
* - Subscription   → rcl/subscription.h
* - Service Client → rcl/client.h
* - Service Server → rcl/service.h
* - Timer          → rcl/timer.h
* - Wait sets      → rcl/wait.h
* - Graph          → rcl/graph.h
* - Init/Shutdown  → rcl/init.h

辅助模块：

4. 核心数据结构

4.1 rcl_context_t — 进程级上下文

1
2
3
4
5
6
7

typedef struct rcl_context_s
{
  /// Global arguments for all nodes which share this context.
  rcl_arguments_t global_arguments;

  /// Implementation specific pointer.
  rcl_context_impl_t * impl;

rcl_context_impl_t 内部持有：

1
2
3
4
5
6
7
8

struct rcl_context_impl_s
{
  rcl_allocator_t allocator;
  rcl_init_options_t init_options;
  int64_t argc;
  char ** argv;
  rmw_context_t rmw_context;
};

生命周期：

1

zero-init → rcl_init() → [valid] → rcl_shutdown() → [invalid] → 销毁所有 entity → rcl_context_fini()

• 一个 context 可创建多个 node（共享同一 rmw_context）
• rcl_shutdown() 后 context 仍”已初始化但无效”，entity 可继续 cleanup
• 支持多 context（多 init 场景，如测试）

4.2 rcl_node_t — 节点

1
2
3
4
5
6
7
8

struct rcl_node_impl_s
{
  rcl_node_options_t options;
  rmw_node_t * rmw_node_handle;
  rcl_guard_condition_t * graph_guard_condition;
  const char * logger_name;
  const char * fq_name;
};

• rmw_node_handle：RMW 层节点句柄
• graph_guard_condition：图变化时唤醒 wait set（新 publisher 出现等）
• logger_name：如 /a/b 命名空间下节点 c → logger 名 a.b.c
• fq_name：完全限定名

4.3 rcl_wait_set_t — 等待集合

1
2
3
4
5
6
7
8

typedef struct rcl_wait_set_s
{
  const rcl_subscription_t ** subscriptions;
  size_t size_of_subscriptions;
  const rcl_guard_condition_t ** guard_conditions;
  // ... timers, clients, services, events
  rcl_wait_set_impl_t * impl;
} rcl_wait_set_t;

内部 rcl_wait_set_impl_t 聚合对应的 rmw_subscriptions_t、rmw_guard_conditions_t 等，最终调用 rmw_wait()。

5. 关键流程

5.1 初始化 — rcl_init()

1
2
3
4
5
6
7
8
9
10
11
12
13

rcl_ret_t rcl_init(int argc, char const * const * argv,
                   const rcl_init_options_t * options, rcl_context_t * context)
{
  // 1. 分配 context->impl
  // 2. 复制 argc/argv
  // 3. rcl_parse_arguments() — 解析 --remap, --params-file, --enclave 等
  // 4. 设置 instance_id（全局唯一）
  // 5. 解析 ROS_DOMAIN_ID（若未指定）
  // 6. 解析 ROS_LOCALHOST_ONLY
  // 7. 设置 enclave 名（默认 "/"）
  // 8. rcl_get_security_options_from_environment()
  // 9. rmw_init() — 初始化中间件
}

要点：

5.2 创建节点 — rcl_node_init()

1
2
3
4
5
6
7
8
9
10
11
12

rcl_ret_t rcl_node_init(rcl_node_t * node, const char * name,
                        const char * namespace_, rcl_context_t * context,
                        const rcl_node_options_t * options)
{
  // 1. 校验 node name / namespace（rmw_validate_*）
  // 2. namespace 规范化（空 → "/"，无前缀 → 加 "/"）
  // 3. remap node name / namespace
  // 4. rmw_create_node()
  // 5. 创建 graph_guard_condition
  // 6. 生成 logger_name, fq_name
  // 7. rcl_logging_rosout_init() — 可选 /rosout 发布
}

5.3 发布 — rcl_publisher_init() + rcl_publish()

初始化：

1
2
3
4
5
6

rcl_ret_t rcl_publisher_init(...)
{
  // 1. rcl_node_resolve_name() — 展开相对名 + remap
  // 2. rmw_create_publisher(node, type_support, remapped_topic, qos)
  // 3. rmw_publisher_get_actual_qos() — 存储实际 QoS
}

发布：

1
2
3
4
5
6
7
8
9
10
11

rcl_ret_t rcl_publish(const rcl_publisher_t * publisher,
                      const void * ros_message,
                      rmw_publisher_allocation_t * allocation)
{
  TRACEPOINT(rcl_publish, ...);
  if (rmw_publish(publisher->impl->rmw_handle, ros_message, allocation) != RMW_RET_OK) {
    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
    return RCL_RET_ERROR;
  }
  return RCL_RET_OK;
}

其他 publish 变体：

5.4 订阅 — rcl_subscription_init() + rcl_take()

取消息：

1
2
3
4
5
6
7
8
9

rcl_ret_t rcl_take(const rcl_subscription_t * subscription,
                   void * ros_message, rmw_message_info_t * message_info,
                   rmw_subscription_allocation_t * allocation)
{
  bool taken = false;
  rmw_ret_t ret = rmw_take_with_info(
    subscription->impl->rmw_handle, ros_message, &taken, message_info_local, allocation);
  // taken == false → RCL_RET_SUBSCRIPTION_TAKE_FAILED
}

5.5 等待与调度 — rcl_wait()

rclcpp::Executor::spin() 底层即循环调用 rcl_wait()：

1
2
3
4
5
6
7
8

rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
{
  // 1. 校验 wait_set 非空
  // 2. 遍历 timers → 计算最近到期时间 → 合并到 timeout
  // 3. 将 timer 的 guard_condition 加入 rmw_guard_conditions
  // 4. rmw_wait(subscriptions, guard_conditions, services, clients, events, timeout)
  // 5. 返回后，未 ready 的 entity 指针置 NULL
}

Wait set 使用模式：

1
2
3
4
5
6
7
8
9
10

rcl_wait_set_t ws = rcl_get_zero_initialized_wait_set();
rcl_wait_set_init(&ws, n_subs, n_gc, n_timers, n_clients, n_services, n_events, context, allocator);

// 循环：
rcl_wait_set_clear(&ws);
rcl_wait_set_add_subscription(&ws, sub);
rcl_wait_set_add_timer(&ws, timer);
rcl_wait(&ws, timeout);           // 阻塞
// 检查 ws.subscriptions[i] 是否仍为 non-NULL → ready
rcl_take(sub, msg, ...);

5.6 Timer

1
2
3
4
5
6
7
8
9
10
11

struct rcl_timer_impl_s
{
  rcl_clock_t * clock;
  rcl_context_t * context;
  rcl_guard_condition_t guard_condition;  // 到期时触发
  atomic_uintptr_t callback;
  atomic_uint_least64_t period;
  atomic_int_least64_t next_call_time;
  atomic_bool canceled;
  rcl_allocator_t allocator;
};

• Timer 不独立线程，依赖 rcl_wait() 检测 guard_condition 或超时
• 支持 RCL_ROS_TIME 跳变（sim time 切换时的 credit 机制）
• rcl_timer_call() 手动触发回调

5.7 Service / Client

与 pub/sub 对称：

5.8 Graph API

路径：src/rcl/graph.c（744 行）

底层全部转发到 rmw_get_* 系列函数。ROS 2 CLI 的 ros2 topic list、ros2 node list 最终依赖这些 API。

6. 参数与 Remap

6.1 arguments.c — CLI 解析核心

最大源文件（2079 行），处理：

Remap 规则解析使用 lexer.c（676 行）+ lexer_lookahead.c 做 token 化。

6.2 rcl_yaml_param_parser

路径：rcl_yaml_param_parser/src/

YAML 格式：

1
2
3
4
5
6

/my_node:
  ros__parameters:
    param1: 42
    param2: "hello"
    nested:
      sub_param: true

解析结果存入 rcl_params_t，由 rcl_arguments 持有，上层（rclcpp）在 node 创建时读取。

7. rcl_action — Action 子包

路径：rcl_action/src/rcl_action/

Action 在 rcl 层不引入新中间件概念，而是将 Action 协议映射为 topics + services 组合：

核心 API：

action_server.c 中通过宏 SERVICE_INIT(Type) 批量创建 send_goal/cancel_goal/get_result 三个 service。

8. rcl_lifecycle — Lifecycle 子包

路径：rcl_lifecycle/src/

提供纯 C 状态机，不含网络通信——上层（rclcpp_lifecycle）负责将状态变化发布到 ~/transition_event topic 和 ~/change_state service。

核心类型：rcl_lifecycle_state_t、rcl_lifecycle_transition_t、rcl_lifecycle_state_machine_t。

9. 错误处理

C 风格，无异常：

常见错误码（types.h）：

10. 源文件布局与规模

10.1 rcl 核心（按行数排序）

10.2 内部头文件（src/rcl/ 私有）

11. 完整数据路径

11.1 发布一条消息

1
2
3
4
5
6

rclcpp::Publisher::publish(msg)
  → rcl_publish(publisher, &msg, allocation)
    → rmw_publish(rmw_handle, &msg, allocation)          [rmw_fastrtps]
      → DataWriter::write(msg)
        → TypeSupport::serialize()                         [Fast-CDR]
          → RTPS 发送

11.2 Executor spin 一次迭代

1
2
3
4
5
6
7
8
9
10
11
12

rclcpp::Executor::spin_some()
  → rcl_wait_set_clear/add_*
  → rcl_wait(wait_set, timeout)
    → rmw_wait(subs, gcs, services, clients, events, timeout)
      → DDS WaitSet 阻塞
  → 对每个 ready subscription:
      → rcl_take(sub, msg, &info, allocation)
        → rmw_take_with_info(...)
  → 对每个 ready timer:
      → rcl_timer_call(timer)
  → 对每个 ready service:
      → rmw_take_request(...) → 用户回调 → rmw_send_response(...)

12. 设计特点小结

13. 与相邻层对比

14. 推荐阅读顺序

1. 总览：include/rcl/rcl.h — 模块地图
2. 生命周期：init.c → context.c → init_options.c
3. 节点：node.c — 理解 namespace/remap/logger
4. 通信：publisher.c + subscription.c — init/publish/take 链路
5. 调度：wait.c + timer.c + guard_condition.c — 理解 spin 底层
6. CLI：arguments.c（可按需跳读）— remap/params 解析
7. 图 API：graph.c
8. 扩展：rcl_action/action_server.c → rcl_lifecycle/rcl_lifecycle.c
9. 上层集成：rclcpp/executor.cpp — 看 rcl_wait 如何被使用

15. API 速查

15.1 最小 C 节点示例

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

#include <rcl/rcl.h>

int main(int argc, char ** argv)
{
  rcl_ret_t ret;
  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
  rcl_init_options_init(&init_options, rcl_get_default_allocator());

  rcl_context_t context = rcl_get_zero_initialized_context();
  ret = rcl_init(argc, argv, &init_options, &context);

  rcl_node_t node = rcl_get_zero_initialized_node();
  rcl_node_options_t node_options = rcl_node_get_default_options();
  rcl_node_init(&node, "my_node", "", &context, &node_options);

  // ... create publisher/subscription, rcl_wait loop ...

  rcl_node_fini(&node);
  rcl_shutdown(&context);
  rcl_context_fini(&context);
  rcl_init_options_fini(&init_options);
  return 0;
}

15.2 关键函数一览

文档基于 ROS 2 Humble 工作区中的 rcl 5.3.13 源码分析生成。

rcpputils 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros2/rcpputils
版本：2.4.6（Humble），单包仓库，构建类型 ament_cmake，语言 C++14，许可证 Apache 2.0 + BSD-3-Clause（部分文件）。

rcpputils 是 ROS 2 C++ 工具库，提供跨平台文件系统、动态库加载、字符串处理、断言/异常、RAII scope guard、线程安全注解等。被 rclcpp、rclpy（C++ 扩展）、rosbag2、rviz2、class_loader 等广泛依赖。设计上 在 rcutils C API 之上提供 C++ 友好封装（异常、std::string、模板），同时补充 rcutils 未覆盖的 C++ 专属能力（std::chrono、type traits、std::filesystem 替代）。

1. 总体认识

1.1 核心职责