一、ROS2 组件化（Composition）

1. ROS2 组件（Component）

ROS2 Component是指把节点做成可动态加载的共享库（.so），不写 main 函数，由一个统一的Component Container（组件容器） 在同一进程里动态加载、运行。

2. 组件容器（Component Container）

容器是一个带 main 的宿主进程，内含 ComponentManager。容器负责加载和卸载组件、创建组件节点对象、统一 Executor调度所有组件的回调、定时器、订阅发布。
三类容器

可执行文件	适用场景
component_container	单线程 executor，简单节点
component_container_mt	多线程，回调可能并行，注意线程安全
component_container_isolated（Humble+）	每个组件自己的 executor，互不抢回调

3. 为什么要组件化

组件化的优点是同一进程内通信可以走 intra-process，不走 DDS 中间件，不用序列化。不用拷贝数据，直接传递指针。

组件化典型收益

• 延迟：相机、IMU、控制环等对周期敏感的链路
• CPU：少一次序列化/反序列化
• 内存：大消息（图像、点云）不必复制多份
• 部署灵活：开发时各开进程方便调试，上机再合成一个进程
• 动态加载：ros2 component load/unload 热插拔节点

4. 组件运行机制

二、ROS2生命周期节点（ROS2 LifecycleNode）

普通节点启动就直接跑，而生命周期节点会把程序拆成一套有限状态机，分阶段初始化、启动、暂停、清理、销毁。每一步都可控、可远程调用、失败可回滚。

1. 普通节点与生命周期节点的核心区别

普通节点：进程一启动，构造函数直接全部初始化，一次性加载硬件、模型、内存资源；要么跑，要么崩，中间不能暂停，资源不能分步释放，外部很难干预内部初始化流程。
生命周期节点：节点进程虽然一直存在，但业务功能不是一上来就运行，必须通过状态切换，分步完成：加载参数→打开硬件→启动业务逻辑→暂停业务→释放硬件资源→最终关闭。每个状态切换都有回调函数，失败可以停在上一状态，不会直接崩溃。自带服务，上位机可以远程发指令切换状态。

2. 生命周期节点解决什么问题

普通 rclcpp::Node / rclpy.Node 只有两种状态：进程在 / 进程死。构造函数一结束、spin() 一开始，定时器就在跑、话题就开始发。

在真机器人上这会出问题：

• 电机驱动还没完成零位标定，步态控制器已经在发力矩。
• 相机还没曝光成功，视觉节点已经在处理空图。
• 想「暂停运动但不杀进程」（改参数、切模式、急停后恢复），只能把节点杀掉再拉起来，连接和状态全丢。
• 十几个节点启动顺序靠 sleep()，时序一抖就崩。
  Lifecycle Node（Managed Node） 给每个节点一套标准状态机。节点自己不当「老板」：外部监督者（CLI、launch、专门的 manager）通过服务驱动它：configure → activate → deactivate → cleanup → shutdown。节点只在 Active 时做主业务。
  Nav2 整栈（planner、controller、costmap、bt_navigator）都是生命周期节点，由 nav2_lifecycle_manager 按依赖顺序拉起。

3. 生命周期节点的四个主状态与六个过渡状态

（1）四个主状态

状态	含义
Unconfigured	刚创建。没分配资源，没建 publisher。cleanup 成功或 on_error 恢复后也会回到这里。
Inactive	已配置：参数读完、接口建好，但不处理业务。LifecyclePublisher 此时发不出去。适合改参、准备硬件，行为还没开始。
Active	真正工作：发数据、控电机、跑规划。
Finalized	终态，即将销毁。方便事后 introspection，不能再配置回去。

（2）六个过渡状态

Configuring 配置中、Activating 激活中、Deactivating 去激活中、Cleaningup清理中、Shuttingdown关闭中、ErrorProcessing 处理错误中。过渡态执行对应的回调函数，如果回调返回成功，就跳到下一个稳态；返回失败，则停留在原来的稳态，不会切换过去。

触发	过渡态	需重写的函数	成功去	失败去
configure	Configuring	on_configure	Inactive	Unconfigured
activate	Activating	on_activate	Active	Inactive
deactivate	Deactivating	on_deactivate	Inactive	Active
cleanup	CleaningUp	on_cleanup	Unconfigured	Inactive
shutdown	ShuttingDown	on_shutdown	Finalized	Finalized
异常 / ERROR	ErrorProcessing	on_error	Unconfigured（可恢复）	Finalized（放弃）

回调函数职责：

• on_configure：一次性准备——读参数、打开串口/CAN、建 publisher/subscription、加载模型、读参数。可以慢。
• on_activate：真正「开始干活」——使能力矩、激活 LifecyclePublisher、启动业务定时器。尽量快。
• on_deactivate：停止业务循环，硬件保持打开——断力矩、停发话题，不要拆掉配置（还要能再 activate）。
• on_cleanup：拆掉 configure 里建的一切，回到「刚 new 出来」的等价状态。
• on_shutdown：从任意主状态来，做最后清理，然后进 Finalized。
• on_error：不知道错在哪一步，必须防御式释放（指针可能半初始化）。

（3）状态流转图

4. 生命周期节点核心优势

• 容错能力强：加载相机 / 模型失败，只停留在未配置，不会整个节点崩溃，方便排查问题。
• 资源精细化管理：机器人待机时，可以 deactivate 到 inactive，算法停止运行，但硬件句柄保留；不用反复打开关闭设备。
• 远程管控标准化：不需要自己写自定义话题，ROS2 自动提供 service 服务，上位机、命令行可以远程发送切换指令。
• 故障恢复：出问题可以 cleanup 清理资源，再重新 configure，实现软重启，不用 kill 整个进程。
• 便于多节点协同：多个驱动节点可以按顺序配置、激活，保证硬件启动时序。

5. 生命周期节点典型应用场景

（1） 硬件驱动

相机、雷达、IMU、电机。configure 打开设备并握手；activate 开始出流；deactivate 停流但不断电；cleanup 关设备。设备没插好就 FAILURE，不要把整个 launch 打死。

（2） 有依赖的启动顺序（四足机器人）

IMU / 雷达 --configure/activate–> 状态估计 --> 运动控制器 --> 电机驱动使能力矩
监督者必须：先让传感器 Active，再 activate 控制器，最后才给电机使能。反过来会「看不见世界就开始踢腿」。

（3）安全暂停，不杀进程

急停、进充电、人靠近：只 deactivate 控制器和电机，传感器继续跑。恢复时再 activate，标定和连接都还在。

（4）在线重配置

deactivate → cleanup → configure（新参数）→ activate。比杀进程重建干净。

（5）Nav2

lifecycle_manager 按列表依次 configure/activate；用 bond 心跳，某个节点死了就把其余的 deactivate，避免规划器还在、控制器已经没了。

6. 示例代码

以四足机械狗为例，构建「电机 + 步态 + 监督者」节点：

• motor_driver：模拟打开总线、零位、使能力矩（只在 Active 时发 joint_torque）。
• gait_controller：只在 Active 时根据假 IMU 算力矩并下发。
• supervisor：严格按 电机 configure → 控制器 configure → 电机 activate → 控制器 activate 顺序运行。急停时先 deactivate 控制器，再 deactivate 电机。

主要示例代码：

（1）motor_driver.py

#!/usr/bin/env python3
"""Lifecycle motor driver: open bus in configure, enable torque only when active."""

import rclpy
from rclpy.executors import SingleThreadedExecutor
from rclpy.lifecycle import Node as LifecycleNode
from rclpy.lifecycle import State, TransitionCallbackReturn
from std_msgs.msg import Float32MultiArray, String


class MotorDriver(LifecycleNode):
    def __init__(self):
        super().__init__('motor_driver')
        self.declare_parameter('num_joints', 12)
        self.declare_parameter('bus_ok', True)  # set false to demo configure failure

        self._num_joints = 12
        self._bus_open = False
        self._torque_enabled = False
        self._cmd_sub = None
        self._status_pub = None
        self._timer = None
        self._last_cmd = None

        self.get_logger().info('motor_driver constructed -> Unconfigured')

    def on_configure(self, state: State) -> TransitionCallbackReturn:
        self._num_joints = int(self.get_parameter('num_joints').value)
        bus_ok = bool(self.get_parameter('bus_ok').value)
        self.get_logger().info(
            f'[configure] from {state.label}: opening bus, joints={self._num_joints}')

        if not bus_ok:
            self.get_logger().error('[configure] bus handshake failed')
            return TransitionCallbackReturn.FAILURE

        # Heavy init lives here (open SPI/CAN, zero joints). Not yet applying torque.
        self._bus_open = True
        self._last_cmd = [0.0] * self._num_joints
        self._status_pub = self.create_lifecycle_publisher(String, 'motor/status', 10)
        self._cmd_sub = self.create_subscription(
            Float32MultiArray, 'motor/cmd_torque', self._on_cmd, 10)
        self.get_logger().info('[configure] bus open, interfaces created -> Inactive')
        return TransitionCallbackReturn.SUCCESS

    def on_activate(self, state: State) -> TransitionCallbackReturn:
        self.get_logger().info(f'[activate] from {state.label}: enabling torque')
        super().on_activate(state)  # activates LifecyclePublisher
        self._torque_enabled = True
        # Create timer only while active so we do not "work" when inactive.
        self._timer = self.create_timer(0.1, self._on_timer)
        return TransitionCallbackReturn.SUCCESS

    def on_deactivate(self, state: State) -> TransitionCallbackReturn:
        self.get_logger().warn(f'[deactivate] from {state.label}: torque OFF, keep bus')
        super().on_deactivate(state)
        self._torque_enabled = False
        if self._timer is not None:
            self._timer.cancel()
            self.destroy_timer(self._timer)
            self._timer = None
        return TransitionCallbackReturn.SUCCESS

    def on_cleanup(self, state: State) -> TransitionCallbackReturn:
        self.get_logger().info(f'[cleanup] from {state.label}: closing bus')
        self._release_all()
        return TransitionCallbackReturn.SUCCESS

    def on_shutdown(self, state: State) -> TransitionCallbackReturn:
        self.get_logger().info(f'[shutdown] from {state.label}')
        self._release_all()
        return TransitionCallbackReturn.SUCCESS

    def on_error(self, state: State) -> TransitionCallbackReturn:
        self.get_logger().error(f'[error] from {state.label}, attempting recovery')
        self._release_all()
        return TransitionCallbackReturn.SUCCESS  # back to Unconfigured

    def _release_all(self):
        self._torque_enabled = False
        self._bus_open = False
        if self._timer is not None:
            self._timer.cancel()
            self.destroy_timer(self._timer)
            self._timer = None
        if self._cmd_sub is not None:
            self.destroy_subscription(self._cmd_sub)
            self._cmd_sub = None
        if self._status_pub is not None:
            self.destroy_publisher(self._status_pub)
            self._status_pub = None

    def _on_cmd(self, msg: Float32MultiArray):
        if not self._torque_enabled:
            return
        self._last_cmd = list(msg.data)

    def _on_timer(self):
        if self._status_pub is None:
            return
        msg = String()
        if self._torque_enabled:
            t0 = self._last_cmd[0] if self._last_cmd else 0.0
            msg.data = f'ENABLED bus=open tau0={t0:.3f}'
        else:
            msg.data = 'DISABLED'
        self._status_pub.publish(msg)
        self.get_logger().info(f'motor: {msg.data}')


def main():
    rclpy.init()
    node = MotorDriver()
    executor = SingleThreadedExecutor()
    executor.add_node(node)
    try:
        executor.spin()
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

（2）supervisor.py

#!/usr/bin/env python3
"""External supervisor: brings nodes up/down in a safe order."""

import rclpy
from rclpy.node import Node
from lifecycle_msgs.srv import ChangeState, GetState
from lifecycle_msgs.msg import Transition, State as LcState
from std_msgs.msg import String


# Must match lifecycle_msgs/msg/Transition.msg
CONFIGURE = Transition.TRANSITION_CONFIGURE      # 1
CLEANUP = Transition.TRANSITION_CLEANUP          # 2
ACTIVATE = Transition.TRANSITION_ACTIVATE        # 3
DEACTIVATE = Transition.TRANSITION_DEACTIVATE    # 4
SHUTDOWN = Transition.TRANSITION_UNCONFIGURED_SHUTDOWN  # 5; service accepts label too


class Supervisor(Node):
    def __init__(self):
        super().__init__('supervisor')
        self.declare_parameter('autostart', True)
        self._motor = 'motor_driver'
        self._gait = 'gait_controller'
        self._clients = {}
        for name in (self._motor, self._gait):
            self._clients[name] = self.create_client(
                ChangeState, f'/{name}/change_state')
        self.create_subscription(String, 'estop', self._on_estop, 10)
        self.get_logger().info('supervisor ready. Publish std_msgs/String data="stop"|"go" on /estop')

        if self.get_parameter('autostart').value:
            self._startup_timer = self.create_timer(1.0, self._do_startup)

    def _do_startup(self):
        self._startup_timer.cancel()
        # Hardware first, then software. Torque enable before gait commands.
        if not self._change(self._motor, CONFIGURE, 'configure'):
            return
        if not self._change(self._gait, CONFIGURE, 'configure'):
            return
        if not self._change(self._motor, ACTIVATE, 'activate'):
            return
        if not self._change(self._gait, ACTIVATE, 'activate'):
            return
        self.get_logger().info('bring-up complete: both nodes Active')

    def _on_estop(self, msg: String):
        cmd = msg.data.strip().lower()
        if cmd == 'stop':
            # Stop commander first, then cut torque.
            self._change(self._gait, DEACTIVATE, 'deactivate')
            self._change(self._motor, DEACTIVATE, 'deactivate')
            self.get_logger().warn('E-STOP: both Inactive, processes still alive')
        elif cmd == 'go':
            self._change(self._motor, ACTIVATE, 'activate')
            self._change(self._gait, ACTIVATE, 'activate')
            self.get_logger().info('resume: both Active')
        elif cmd == 'shutdown':
            self._change(self._gait, DEACTIVATE, 'deactivate')
            self._change(self._motor, DEACTIVATE, 'deactivate')
            self._change(self._gait, 7, 'shutdown')   # TRANSITION_ACTIVE_SHUTDOWN=7 if still active
            # Prefer label-based call via helper below for shutdown from any state
            self._shutdown(self._gait)
            self._shutdown(self._motor)

    def _shutdown(self, node_name: str) -> bool:
        # Try the three shutdown IDs; only one is valid from current primary state.
        for tid in (
            Transition.TRANSITION_ACTIVE_SHUTDOWN,
            Transition.TRANSITION_INACTIVE_SHUTDOWN,
            Transition.TRANSITION_UNCONFIGURED_SHUTDOWN,
        ):
            if self._change(node_name, tid, 'shutdown', quiet=True):
                return True
        self.get_logger().error(f'shutdown {node_name} failed')
        return False

    def _change(self, node_name: str, transition_id: int, label: str, quiet=False) -> bool:
        client = self._clients[node_name]
        if not client.wait_for_service(timeout_sec=5.0):
            self.get_logger().error(f'{node_name}/change_state not available')
            return False
        req = ChangeState.Request()
        req.transition.id = transition_id
        req.transition.label = label
        future = client.call_async(req)
        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
        if not future.result() or not future.result().success:
            if not quiet:
                self.get_logger().error(f'{node_name} {label} failed')
            return False
        self.get_logger().info(f'{node_name}: {label} OK')
        return True


def main():
    rclpy.init()
    node = Supervisor()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

（3）完整项目结构