一、概述

本章对应手册第十一章，内容为整机层面的状态与安全接口，不涉及单个关节或外设。这些接口回答四个问题：机器人当前是否可运动、是否处于急停、是否有待处理的关机请求、自检结果是否正常。

与其他外设章节不同，本章接口属于接入程序的必备内容。手册 1.2 节的接入步骤共 7 条，第 2 条为安全相关要求，也是 7 条中唯一使用"始终"措辞的一条：

该条属于强制要求。若程序不检查急停状态，急停触发后仍会继续下发运动命令：执行器会进入安全状态，但程序内部的运动逻辑与实际状态不再一致。

1.1 本章接口范围

另有服务两个：整机自检 /fd_robot/self_check，遥操作会话开关 /fd_robot/teleoperation/control。

1.2 信号方向

手册接口清单表的「方向」列以接入程序为视角。2.1 节原文：「"发布"表示由你的程序发送命令，"订阅"表示由你的程序接收反馈」。

1.3 本章不覆盖的内容

手册原文：「参数库、License、固件升级、SQLite 和硬件诊断的细节不在本章范围内。」附录「未收录的接口」列出了以下路径：

整机健康检查的入口为 /fd_robot/self_check。查询单一子系统时使用 /fd_robot/self_check/<name>。其中 arm 与 chassis 两项系统中没有节点提供，超时或失败属于正常现象，不应在代码中按失败处理。

1.4 与各外设章节的关系

急停属于整机信号，作用对象是具体执行器。急停触发后各执行器的行为分散在对应章节，本章只说明信号本身。

1.5 JSON 字段

二、运行前提与约定

2.1 通用运行前提

2.2 三条强制约定

2.3 QoS

手册第十一章未对任何话题标注 QoS 要求，使用默认 QoS（RELIABLE / VOLATILE，深度 10）即可。这与腰部 YD100 的三个 BEST_EFFORT 状态话题不同，不宜照搬。全手册只有腰部 YD100 的 joint_state / cartesian_state / motion_status 明确要求 BEST_EFFORT。

2.4 频率一览

三、接口总览

3.1 二次开发接口（4 话题 + 2 服务）

手册 11.1 / 11.2 节共列出 6 个二次开发接口，没有动作（11.2 节只列服务）。

四个话题的方向列全部为「订阅」，手册中仅本章如此：整机状态由系统上报，接入程序只负责接收。本章没有命令类话题，可由接入程序主动发起的只有两个服务（整机自检、遥操作开关）。

3.2 系统内部接口（9 话题）

手册 11.1 节在"系统内部接口"标签下列了 9 条。它们由底层使用，列出仅供排查问题时参考，不应与二次开发接口同时调用（手册 2.1 节）。

手册原文：「Pico 输入由遥操作发布端发出，身体控制节点订阅后将其转换为头颈、腰部、升降等控制命令。」这三条 Pico 话题有数据，说明遥操作可能正在控制机器人；此时按 1.2 节第 6 条不应再通过本手册接口下发命令。它们可作为判断是否存在遥控操作的旁证。

3.3 动作（Action）

3.4 各子系统自检服务一览

手册 11.2 节原文：「各子系统另有独立的自检服务 /fd_robot/self_check/<name>，见对应章节。」这些服务全部使用同一个类型 fd_msgs/SelfCheckSrv，散落在各章中，此处做一次跨章汇总。

arm / chassis 自检查询不到属于正常情况：汇总自检会调用这两个服务并因此超时，超时不代表故障。手册第四章原文：「hh_servo：HH 舵机，不支持自检、声源跟随和 dm_motor 底层命令。」即 head_* 系列在 hh_servo 下不支持自检。手册第五章原文：「GiantCrab 不提供 zerr 方案中的使能、速度控制、参数读写和自检接口。」GiantCrab 腰部同样没有自检。

四、话题接口详解

本章按手册 11.1 节的顺序，逐个说明 4 个二次开发话题。每个接口固定给出「语义卡片 → 字段表 → 三段示例（命令行 / Python / C++）」。

关于 QoS：手册第十一章未标注 QoS，使用默认 QoS（RELIABLE / VOLATILE，深度 10）即可。

本章示例均为示意代码。robot_status / robot_idle_status 的 JSON 字段手册未给出，示例只处理手册明确提到过的 idle 字段，其余字段按原文打印，不做推测。

字段表

该话题不宜按需订阅。手册未标注本话题频率（2.1 节：未注明频率的话题为按需发布）。若采用按需 sub 的写法，create_subscription 之后的第一条消息可能要等到下一次状态变化才到达，订阅之前发生的急停会被漏掉。正确做法是节点初始化时即订阅，将 data 缓存到成员变量，发命令前检查缓存值。

示例

① 命令行（ros2 CLI）

# 持续监听急停状态（最基础的用法，先确认话题存在）
ros2 topic echo /fd_robot/emergency_signal

# 只看 data 字段，便于肉眼扫
ros2 topic echo --field data /fd_robot/emergency_signal

# 确认发布者（可能有多个发布源）与类型
ros2 topic info -v /fd_robot/emergency_signal
ros2 interface show std_msgs/msg/Bool

# 看实际发布频率（手册未标频率，用这个判断是电平型还是事件型）
ros2 topic hz /fd_robot/emergency_signal

# ✗ 错误用法：不要用 --once 做"检查一次"——没消息时会一直阻塞
# ros2 topic echo --once /fd_robot/emergency_signal

② Python（rclpy）

# 示意代码：急停守护——常驻订阅 + 锁存 + 发命令前检查
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool
from sensor_msgs.msg import JointState

class SafetyMonitor(Node):
    """把急停状态集中管起来，所有发命令的地方都先问它。"""

    def __init__(self):
        super().__init__('safety_monitor')
        # 关键：初始化时就订阅，不要等要用的时候才订阅
        self.create_subscription(
            Bool, '/fd_robot/emergency_signal', self.on_emergency, 10)
        self._estop = False          # 锁存：一旦 true 就保持，直到人工复位
        self._seen = False           # 是否收到过任何一条消息

    def on_emergency(self, msg):
        self._seen = True
        if msg.data:
            if not self._estop:
                self.get_logger().error('急停触发：停止发送任何运动命令')
            self._estop = True       # 只置位，不自动清除
        else:
            # 手册未说明 false = 已解除，因此这里只记录、不自动复位
            self.get_logger().warn('收到 emergency_signal=false（不自动解除锁存）')

    @property
    def estop(self):
        return self._estop

    def reset(self, confirmed=False):
        """人工确认后才允许复位；程序里不要自动调用"""
        if confirmed:
            self._estop = False
            self.get_logger().warn('急停锁存已被人工复位')

    def guard(self):
        """发命令前的统一闸门：返回 True 表示允许发送"""
        if not self._seen:
            self.get_logger().warn('尚未收到急停信号，谨慎发送命令')
        return not self._estop

class SafePublisher(Node):
    def __init__(self, safety):
        super().__init__('safe_publisher')
        self.safety = safety
        self.pub = self.create_publisher(
            JointState, '/fd_robot/manipulator/joint_state_cmd', 10)

    def send(self, name, position):
        if not self.safety.guard():          # 手册 1.2 节第 2 条
            self.get_logger().error('急停中，拒绝发送命令')
            return False
        m = JointState()
        m.name = [name]
        m.position = [float(position)]
        self.pub.publish(m)
        return True

def main():
    rclpy.init()
    safety = SafetyMonitor()
    pub = SafePublisher(safety)

    # 先 spin 一会儿，把急停状态拿到手
    for _ in range(10):
        rclpy.spin_once(safety, timeout_sec=0.1)

    pub.send('left_jaw', 800.0)

    # 运行期间持续 spin，保证急停回调随时能被触发
    try:
        rclpy.spin(safety)
    except KeyboardInterrupt:
        pass
    finally:
        safety.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <atomic>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/bool.hpp"

class SafetyMonitor : public rclcpp::Node {
public:
  SafetyMonitor() : Node("safety_monitor") {
    // 关键：初始化时就订阅
    sub_ = create_subscription<std_msgs::msg::Bool>(
        "/fd_robot/emergency_signal", 10,
        [this](std_msgs::msg::Bool::SharedPtr m) {
          seen_ = true;
          if (m->data) {
            if (!estop_.exchange(true))
              RCLCPP_ERROR(get_logger(), "急停触发：停止发送任何运动命令");
          } else {
            // 手册未说明 false = 已解除，只记录、不自动复位
            RCLCPP_WARN(get_logger(),
                        "收到 emergency_signal=false（不自动解除锁存）");
          }
        });
  }

  bool guard() const {
    if (!seen_) RCLCPP_WARN(get_logger(), "尚未收到急停信号，谨慎发送命令");
    return !estop_.load();                 // 手册 1.2 节第 2 条
  }

  void reset(bool confirmed) {             // 仅人工确认后调用
    if (confirmed) {
      estop_.store(false);
      RCLCPP_WARN(get_logger(), "急停锁存已被人工复位");
    }
  }

private:
  std::atomic<bool> estop_{false};
  std::atomic<bool> seen_{false};
  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<SafetyMonitor>();

  // 先 spin 一段时间，把急停状态拿到手
  auto warmup = node->now() + std::chrono::milliseconds(500);
  while (rclcpp::ok() && node->now() < warmup) rclcpp::spin_some(node);

  if (node->guard()) {
    RCLCPP_INFO(node->get_logger(), "允许发送运动命令");
  } else {
    RCLCPP_ERROR(node->get_logger(), "急停中，拒绝发送命令");
  }

  rclcpp::spin(node);                      // 常驻，保证回调随时可达
  rclcpp::shutdown();
  return 0;
}

字段表

同一话题名对应两个不同的来源，语义也不完全等价：

程序只能看到话题上有一条 true，无法仅凭话题名分辨来源。手册未给出区分手段。保守做法是一律按「机器人将关机」处理：停止业务、保存状态、不再下发新命令。示例将两种情况都记入日志，便于事后排查。

示例

① 命令行（ros2 CLI）

# 监听关机请求
ros2 topic echo /fd_robot/shutdown_signal

# 确认有几个发布者——若是 woosh 底盘，可能不止一个来源
ros2 topic info -v /fd_robot/shutdown_signal

# 对比第七章的同名话题（woosh 专用），看发布者是否不同
ros2 topic info -v /fd_robot/shutdown_signal

# 查看类型定义
ros2 interface show std_msgs/msg/Bool

② Python（rclpy）

# 示意代码：监听关机请求，与急停同样"锁存 + 不自动复位"
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool

class ShutdownWatcher(Node):
    def __init__(self):
        super().__init__('shutdown_watcher')
        # 注意：整机（电源板）与 woosh 底盘可能共用这一条话题
        self.create_subscription(
            Bool, '/fd_robot/shutdown_signal', self.on_shutdown, 10)
        self._requested = False
        self._count = 0

    def on_shutdown(self, msg):
        self._count += 1
        if msg.data:
            if not self._requested:
                self.get_logger().error(
                    '收到关机请求：停止业务、保存状态、不再下发命令')
            self._requested = True
        else:
            # 手册未说明 false 的含义，只记录
            self.get_logger().warn('收到 shutdown_signal=false')

    @property
    def requested(self):
        return self._requested

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

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <atomic>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/bool.hpp"

class ShutdownWatcher : public rclcpp::Node {
public:
  ShutdownWatcher() : Node("shutdown_watcher") {
    // 整机（电源板）与 woosh 底盘可能共用这一条话题
    sub_ = create_subscription<std_msgs::msg::Bool>(
        "/fd_robot/shutdown_signal", 10,
        [this](std_msgs::msg::Bool::SharedPtr m) {
          if (m->data) {
            if (!requested_.exchange(true))
              RCLCPP_ERROR(get_logger(),
                           "收到关机请求：停止业务、保存状态、不再下发命令");
          } else {
            RCLCPP_WARN(get_logger(), "收到 shutdown_signal=false");
          }
        });
  }
  bool requested() const { return requested_.load(); }

private:
  std::atomic<bool> requested_{false};
  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ShutdownWatcher>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

字段表

频率推断：/diagnostics 是内部接口，约 0.5 Hz，而 robot_status 手册说明「随 /diagnostics 更新」，因此其更新频率大致也在秒级，不适合用于高频状态轮询；需要高频判断时请使用具体外设的状态话题。该结论依据手册两句话推出，不是手册直接标注的频率。

示例

① 命令行（ros2 CLI）

# 第一步永远是：看实际 JSON 长什么样
ros2 topic echo /fd_robot/robot_status

# 只看 data 字段，便于接管道处理
ros2 topic echo --field data /fd_robot/robot_status

# 配合 jq 做临时解析（需本机装了 jq；字段名以实际输出为准）
ros2 topic echo --field data --once /fd_robot/robot_status | jq .

# 看更新频率（判断是否秒级）
ros2 topic hz /fd_robot/robot_status

# 上游来源（内部接口，仅供排查）
ros2 topic echo /diagnostics

② Python（rclpy）

# 示意代码：容错解析 robot_status——字段名以 ros2 topic echo 实际输出为准
import json
import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class RobotStatus(Node):
    def __init__(self):
        super().__init__('robot_status_reader')
        self.create_subscription(
            String, '/fd_robot/robot_status', self.on_status, 10)
        self.raw = None
        self.data = None

    def on_status(self, msg):
        self.raw = msg.data
        try:
            self.data = json.loads(msg.data)
        except json.JSONDecodeError as e:
            # 不要因为一次解析失败就崩掉整个节点
            self.get_logger().warn(f'JSON 解析失败: {e}')
            self.data = None
            return
        # 手册未给字段清单 —— 这里只做"打印 + 递归收集异常项"，
        # 不硬编码任何字段名
        self.get_logger().info(f'robot_status 顶层键: {list(self.data)}')

    def find_abnormal(self, node=None, path=''):
        """递归找出看起来像"异常"的条目，仅用于人工排查；
           不依赖任何固定的字段名"""
        node = self.data if node is None else node
        hits = []
        if isinstance(node, dict):
            for k, v in node.items():
                hits += self.find_abnormal(v, f'{path}.{k}')
        elif isinstance(node, list):
            for i, v in enumerate(node):
                hits += self.find_abnormal(v, f'{path}[{i}]')
        else:
            s = str(node).lower()
            if any(w in s for w in ('error', 'warn', 'abnormal', 'fail')):
                hits.append(f'{path} = {node}')
        return hits

def main():
    rclpy.init()
    node = RobotStatus()
    for _ in range(20):
        rclpy.spin_once(node, timeout_sec=0.5)
    if node.data is None:
        node.get_logger().warn('未收到可解析的 robot_status')
    else:
        for h in node.find_abnormal():
            node.get_logger().warn(h)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <memory>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/string.hpp"

// 注意：C++ 标准库没有内置 JSON。示例只做"原样打印 + 关键词粗筛"，
// 需要结构化解析请引入 nlohmann/json 等第三方库
class RobotStatus : public rclcpp::Node {
public:
  RobotStatus() : Node("robot_status_reader") {
    sub_ = create_subscription<std_msgs::msg::String>(
        "/fd_robot/robot_status", 10,
        [this](std_msgs::msg::String::SharedPtr m) {
          raw_ = m->data;
          RCLCPP_INFO(get_logger(), "robot_status: %s", raw_.c_str());
          // 关键词粗筛，仅用于人工排查；字段名以实际输出为准
          for (const char* w : {"error", "warn", "abnormal", "fail"}) {
            if (raw_.find(w) != std::string::npos)
              RCLCPP_WARN(get_logger(), "命中关键词: %s", w);
          }
        });
  }
  const std::string& raw() const { return raw_; }

private:
  std::string raw_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<RobotStatus>());
  rclcpp::shutdown();
  return 0;
}

字段表

「整机空闲」的判断在本版本中不能只依赖这一个话题，手册给出的组合如下：

nav_status 的类型是自定义消息 fd_msgs/ChassisNavStatusMsg（三个 bool 标志），不是字符串，不应按 JSON 解析。该交叉验证需在底盘章节的接口类型下实现，本文只给出联动的判定思路，完整字段与判定时机见 《天链机器人底盘接口说明》。

关于 idle 字段的类型：手册只说明"不要只看 idle 字段"，没有说明它是布尔还是字符串。示例采用宽松判断（同时接受 true、"true"、1、"idle" 等写法），不硬断言其为 bool。

示例

① 命令行（ros2 CLI）

# 先看 JSON 实际结构（重点确认 idle 字段的类型）
ros2 topic echo /fd_robot/robot_idle_status

# 2 Hz，可用 hz 确认
ros2 topic hz /fd_robot/robot_idle_status

# 只看 data
ros2 topic echo --field data /fd_robot/robot_idle_status

# 底盘侧交叉验证（第七章）
ros2 topic echo /fd_robot/chassis/nav_status
ros2 topic echo /fd_robot/chassis/current_pose

② Python（rclpy）

# 示意代码：空闲判断 = idle 字段 + 底盘 nav_status 交叉验证
import json
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from fd_msgs.msg import ChassisNavStatusMsg   # 注意：nav_status 不是字符串

IDLE_TRUE = {True, 1, 'true', 'True', 'idle', 'IDLE'}

def as_idle(value):
    """宽松判断：手册未说明 idle 字段的类型，避免硬断言"""
    if isinstance(value, bool):
        return value
    if isinstance(value, (int, float)):
        return value != 0
    if isinstance(value, str):
        return value in IDLE_TRUE
    return None                      # 无法判断

class IdleChecker(Node):
    def __init__(self):
        super().__init__('idle_checker')
        self.create_subscription(
            String, '/fd_robot/robot_idle_status', self.on_idle, 10)
        # 手册：底盘运动可能未计入，需结合 nav_status
        self.create_subscription(
            ChassisNavStatusMsg, '/fd_robot/chassis/nav_status',
            self.on_nav, 10)
        self.idle = None
        self.nav_active = False

    def on_idle(self, msg):
        try:
            self.idle = json.loads(msg.data)
        except json.JSONDecodeError:
            self.idle = None

    def on_nav(self, msg):
        self.nav_active = bool(msg.active)   # 自定义消息，三个 bool 标志

    def is_really_idle(self):
        """两路信号都空闲才算空闲（底盘导航 active 则不算空闲）"""
        raw_idle = self.idle.get('idle') if isinstance(self.idle, dict) else None
        idle_ok = as_idle(raw_idle)
        if idle_ok is not True:
            return False, f'idle 字段未表明空闲: {raw_idle}'
        if self.nav_active:
            return False, '底盘导航 active，不算空闲'
        return True, 'ok'

def main():
    rclpy.init()
    node = IdleChecker()
    for _ in range(20):
        rclpy.spin_once(node, timeout_sec=0.2)
    ok, why = node.is_really_idle()
    node.get_logger().info(f'整机空闲: {ok} ({why})')
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <memory>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/string.hpp"
#include "fd_msgs/msg/chassis_nav_status_msg.hpp"   // nav_status 不是字符串

// 示例只对 JSON 做关键词粗筛；结构化解析请引入第三方 JSON 库。
// 手册未公开 idle 字段的类型，这里不硬断言 bool
class IdleChecker : public rclcpp::Node {
public:
  IdleChecker() : Node("idle_checker") {
    idle_sub_ = create_subscription<std_msgs::msg::String>(
        "/fd_robot/robot_idle_status", 10,
        [this](std_msgs::msg::String::SharedPtr m) { idle_raw_ = m->data; });
    nav_sub_ = create_subscription<fd_msgs::msg::ChassisNavStatusMsg>(
        "/fd_robot/chassis/nav_status", 10,
        [this](fd_msgs::msg::ChassisNavStatusMsg::SharedPtr m) {
          nav_active_ = m->active;      // 自定义消息，三个 bool 标志
        });
  }

  bool idle_from_topic() const {
    // 宽松匹配，不假设 idle 是 bool 还是 string
    return idle_raw_.find("true") != std::string::npos ||
           idle_raw_.find("idle") != std::string::npos;
  }
  bool really_idle() const { return idle_from_topic() && !nav_active_; }

private:
  std::string idle_raw_;
  bool nav_active_ = false;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr idle_sub_;
  rclcpp::Subscription<fd_msgs::msg::ChassisNavStatusMsg>::SharedPtr nav_sub_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<IdleChecker>();
  auto end = node->now() + std::chrono::seconds(2);
  while (rclcpp::ok() && node->now() < end) rclcpp::spin_some(node);
  RCLCPP_INFO(node->get_logger(), "整机空闲: %s",
              node->really_idle() ? "true" : "false");
  rclcpp::shutdown();
  return 0;
}

五、服务接口详解

本章说明手册 11.2 节的 2 个服务。这两个服务是整机层面唯一可由接入程序主动发起的调用：执行一次健康检查，或开关遥操作会话。

字段表（全章自检服务通用）

手册 11.3 节原文：「SelfCheckSrv（/fd_robot/self_check 及各子系统自检服务通用）」。SelfCheckSrv 这一张字段表适用于本章整机自检以及第三章 3.4 节列出的全部子系统自检服务。

check_abnormal_info 为空表示该项正常，非空表示异常，不应理解为「返回的列表即检查项」。该接口只报异常：正常时数组为空，看起来像未返回内容。

示例

① 命令行（ros2 CLI）

# 整机自检汇总（注意：可能因调用不存在的 arm / chassis 服务而超时）
ros2 service call /fd_robot/self_check fd_msgs/srv/SelfCheckSrv "{}"

# 自检可能很慢，加个外层超时，避免一直卡住
timeout 60 ros2 service call /fd_robot/self_check fd_msgs/srv/SelfCheckSrv "{}"

# 更推荐：直接查具体子系统（语义明确、不会假超时）
ros2 service call /fd_robot/self_check/lifting_motor fd_msgs/srv/SelfCheckSrv "{}"
ros2 service call /fd_robot/self_check/battery  fd_msgs/srv/SelfCheckSrv "{}"
ros2 service call /fd_robot/self_check/head     fd_msgs/srv/SelfCheckSrv "{}"

# 查看服务类型定义
ros2 interface show fd_msgs/srv/SelfCheckSrv

# ✗ 这两个服务系统中没有节点提供，查了必然失败/超时（属正常现象）
# ros2 service call /fd_robot/self_check/arm     fd_msgs/srv/SelfCheckSrv "{}"
# ros2 service call /fd_robot/self_check/chassis fd_msgs/srv/SelfCheckSrv "{}"

② Python（rclpy）

# 示意代码：带充足超时的自检调用；check_abnormal_info 非空才报异常
import time
import rclpy
from rclpy.node import Node
from fd_msgs.srv import SelfCheckSrv

# 汇总自检可能因缺少 arm / chassis 服务而很慢，超时要给足
SELF_CHECK_TIMEOUT = 30.0

def check(node, service):
    cli = node.create_client(SelfCheckSrv, service)
    if not cli.wait_for_service(timeout_sec=5.0):
        node.get_logger().warn(f'{service} 服务不可用')
        return None
    fut = cli.call_async(SelfCheckSrv.Request())     # 无请求字段
    rclpy.spin_until_future_complete(
        node, fut, timeout_sec=SELF_CHECK_TIMEOUT)
    if not fut.done():
        node.get_logger().warn(f'{service} 超时（汇总自检属常见现象）')
        return None
    return fut.result()

def report(node, res, service):
    if res is None:
        return
    # 空数组 = 正常；非空 = 异常说明
    if not res.check_abnormal_info:
        node.get_logger().info(f'[{service}] name={res.name} 正常')
    else:
        for info in res.check_abnormal_info:
            node.get_logger().error(f'[{service}] name={res.name} 异常: {info}')

def main():
    rclpy.init()
    node = Node('self_check_client')

    # 优先查具体子系统：语义明确，不受缺失服务影响
    for svc in ('/fd_robot/self_check/lifting_motor',
                '/fd_robot/self_check/battery'):
        report(node, check(node, svc), svc)

    # 整机汇总放最后，且单独容错
    report(node, check(node, '/fd_robot/self_check'),
           '/fd_robot/self_check')

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <chrono>
#include <memory>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include "fd_msgs/srv/self_check_srv.hpp"

using namespace std::chrono_literals;
using SelfCheckSrv = fd_msgs::srv::SelfCheckSrv;

// 汇总自检可能因缺少 arm / chassis 服务而很慢，超时要给足
static constexpr auto kTimeout = 30s;

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("self_check_client");

  auto run = [&node](const std::string& service) {
    auto cli = node->create_client<SelfCheckSrv>(service);
    if (!cli->wait_for_service(5s)) {
      RCLCPP_WARN(node->get_logger(), "%s 服务不可用", service.c_str());
      return;
    }
    auto req = std::make_shared<SelfCheckSrv::Request>();   // 无请求字段
    auto fut = cli->async_send_request(req);
    if (rclcpp::spin_until_future_complete(node, fut, kTimeout) !=
        rclcpp::FutureReturnCode::SUCCESS) {
      RCLCPP_WARN(node->get_logger(), "%s 超时（汇总自检属常见现象）",
                  service.c_str());
      return;
    }
    const auto res = fut.get();
    if (res->check_abnormal_info.empty()) {      // 空 = 正常
      RCLCPP_INFO(node->get_logger(), "[%s] name=%s 正常",
                  service.c_str(), res->name.c_str());
    } else {
      for (const auto& info : res->check_abnormal_info)
        RCLCPP_ERROR(node->get_logger(), "[%s] name=%s 异常: %s",
                     service.c_str(), res->name.c_str(), info.c_str());
    }
  };

  // 优先查具体子系统
  run("/fd_robot/self_check/lifting_motor");
  run("/fd_robot/self_check/battery");
  // 整机汇总放最后
  run("/fd_robot/self_check");

  rclcpp::shutdown();
  return 0;
}

字段表

status=1 不等于状态发生了改变。手册明确 1 包含 already start / already stop：调用 start 而会话本来已开启时同样返回 1。判断会话最终是否开启需读取 message。status=-1 也不一定代表命令取值错误，未授权时同样返回 −1，此时 message 为「遥操功能未授权」。两种情况都应先读取 message 再判断。

开/关会话的连带影响

示例

① 命令行（ros2 CLI）

# 关闭遥操作会话
ros2 service call /fd_robot/teleoperation/control fd_msgs/srv/TeleoperationControl \
  "{command: 'stop'}"

# 开启本机 Pico 会话（需要 teleoperation 功能授权）
ros2 service call /fd_robot/teleoperation/control fd_msgs/srv/TeleoperationControl \
  "{command: 'start'}"

# 开启云端会话
ros2 service call /fd_robot/teleoperation/control fd_msgs/srv/TeleoperationControl \
  "{command: 'start_cloud'}"

# 返回示例解读：
#   status=1,  message='ok'             -> 本次真的切换了状态
#   status=1,  message='already start'  -> 本来就已经开着（status=1 不代表这次改了）
#   status=-1, message='遥操功能未授权'  -> 没 License，不是命令写错
ros2 interface show fd_msgs/srv/TeleoperationControl

② Python（rclpy）

# 示意代码：开关遥操作会话，按 message 而不是 status 判断真实结果
import rclpy
from rclpy.node import Node
from fd_msgs.srv import TeleoperationControl

def control(node, command, timeout=5.0):
    """command: start / start_cloud / stop；其他值会返回失败"""
    cli = node.create_client(
        TeleoperationControl, '/fd_robot/teleoperation/control')
    if not cli.wait_for_service(timeout_sec=timeout):
        node.get_logger().error('遥操作管理服务不可用')
        return None

    req = TeleoperationControl.Request()
    req.command = command
    fut = cli.call_async(req)
    rclpy.spin_until_future_complete(node, fut, timeout_sec=timeout)
    if not fut.done():
        node.get_logger().error('调用超时')
        return None

    res = fut.result()
    # status=1 包含 already start / already stop，因此必须看 message
    if res.status == 1:
        node.get_logger().info(
            f'command={command} 成功，message={res.message}')
    else:
        # status=-1：可能未授权，也可能 command 取值非法
        node.get_logger().error(
            f'command={command} 失败，status={res.status} '
            f'message={res.message}')
    return res

def main():
    rclpy.init()
    node = Node('teleop_control_client')

    # 关闭会话（手册未说明 stop 是否免授权，因此同样要检查返回）
    control(node, 'stop')

    # 开启本机 Pico 会话
    res = control(node, 'start')
    if res is not None and res.status != 1:
        node.get_logger().warn('会话未开启，后续不要依赖 Pico 相关话题')

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <chrono>
#include <memory>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include "fd_msgs/srv/teleoperation_control.hpp"

using namespace std::chrono_literals;
using TeleoperationControl = fd_msgs::srv::TeleoperationControl;

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("teleop_control_client");

  auto cli = node->create_client<TeleoperationControl>(
      "/fd_robot/teleoperation/control");
  if (!cli->wait_for_service(5s)) {
    RCLCPP_ERROR(node->get_logger(), "遥操作管理服务不可用");
    rclcpp::shutdown();
    return 1;
  }

  auto control = [&node, &cli](const std::string& command) {
    auto req = std::make_shared<TeleoperationControl::Request>();
    req->command = command;        // start / start_cloud / stop
    auto fut = cli->async_send_request(req);
    if (rclcpp::spin_until_future_complete(node, fut, 5s) !=
        rclcpp::FutureReturnCode::SUCCESS) {
      RCLCPP_ERROR(node->get_logger(), "调用超时");
      return false;
    }
    const auto res = fut.get();
    // status=1 包含 already start / already stop，必须看 message
    if (res->status == 1) {
      RCLCPP_INFO(node->get_logger(), "command=%s 成功 message=%s",
                  command.c_str(), res->message.c_str());
      return true;
    }
    // status=-1：可能未授权，也可能 command 取值非法
    RCLCPP_ERROR(node->get_logger(), "command=%s 失败 status=%d message=%s",
                 command.c_str(), res->status, res->message.c_str());
    return false;
  };

  control("stop");
  if (!control("start"))
    RCLCPP_WARN(node->get_logger(), "会话未开启，勿依赖 Pico 话题");

  rclcpp::shutdown();
  return 0;
}

六、系统内部接口

手册 11.1 节在"系统内部接口"标签下列了 9 条。它们由驱动、诊断和遥操作底层使用，列出仅供排查问题时参考，不应与二次开发接口同时调用（手册 2.1 节）。本节不逐一展开字段，需要时对类型名执行 ros2 interface show。

这 9 条中有 2 条发布后会改变机器人状态（见 6.1 节），其余 7 条为只读的观察类接口，用于排障。

6.1 危险组：不应主动发布

6.2 健康观察组

main_board_status 的字段名手册只给出前缀。手册原文是「字段包括 cpu_*、mem_*、disk_home_*、disk_etc_*」，为通配写法，说明同一前缀下有多个字段（例如 cpu 使用率、cpu 温度等）。具体字段名可用 ros2 interface show fd_msgs/msg/BoardStateMsg 查看，本文档不补充。

/diagnostics 是 ROS 标准接口，可以复用现成工具。其类型为 diagnostic_msgs/DiagnosticArray，标准生态中的诊断工具（如 rqt_robot_monitor）可直接查看。/fd_robot/robot_status 是将其转为 JSON 后的汇总，两者是上游与下游关系，不是两份独立数据。

6.3 Pico 遥操作输入组

配置条件统一为「Pico 遥操作发布端运行中」。手册 11.1 节补充：「Pico 输入由遥操作发布端发出，身体控制节点订阅后将其转换为头颈、腰部、升降等控制命令。」

排障用途：这三条话题有数据说明遥操作链路处于活动状态。结合 1.2 节第 6 条（同一执行器同一时间只能有一个控制来源），若在调试机械臂时这三条话题有数据，说明可能正在使用 Pico 遥控，此时不应再通过本手册接口下发命令。这些话题只在会话开启后才会出现（11.3 节），因此话题不存在通常等于遥操作会话未开启。

6.4 内部接口排查示例

内部接口不写三段示例（它们是排查工具，不写进程序）。下面这组命令覆盖了实际排障时最常用的查法。

# ① 主板资源：看 CPU / 内存 / 磁盘是否吃紧（1 Hz）
ros2 topic echo /fd_robot/main_board_status
ros2 interface show fd_msgs/msg/BoardStateMsg    # 查具体字段名

# ② 电源板：电流 / 电压 / 温度
ros2 topic echo /fd_robot/power_board/status

# ③ ROS 标准诊断（约 0.5 Hz）
ros2 topic echo /diagnostics
ros2 topic hz /diagnostics

# ④ 时间同步（多见于 x86 平台）
ros2 topic echo /fd_time_sync/trigger_sync

# ⑤ 确认 Pico 遥操作链路是否活跃（有数据 = 可能有人在遥控）
ros2 topic list | grep pico
ros2 topic hz /fd_robot/pico/joystick

# ⑥ 查看这些内部话题的发布者，判断是哪个节点在提供
ros2 topic info -v /fd_robot/main_board_status
ros2 topic info -v /fd_robot/power_board/status

# ✗ 以下两条危险，仅供确认存在性，不要真的发布
# ros2 topic pub --once /fd_robot/reboot_service_signal std_msgs/msg/Bool "{data: true}"
# ros2 topic pub --once /fd_robot/service_control_signal fd_msgs/msg/ServiceControlMsg \
#   "{action: 'restart', target: 'target_brain'}"

代码块中最后两条被注释的命令仅用于确认话题存在。手册明确规定这两个话题不作为常规控制使用，其语义是重启服务，一条消息即可中断整个系统。接入程序中若出现这两个话题名，通常属于设计错误。

七、典型流程与完整示例

7.1 安全监控的标准架构

本章接口的推荐用法是收敛为一个统一的闸门：所有发送运动命令的代码路径都经过它，而不是在每个业务函数中各自订阅一次。

7.2 三个信号的正确反应

7.3 Python 端到端示例：安全守护 + 状态汇总

# 示意代码：把急停 / 关机 / 空闲 / 诊断汇总收敛成一个守护节点
import json
import threading
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from std_msgs.msg import Bool, String

IDLE_TRUE = {True, 1, 'true', 'True', 'idle', 'IDLE'}

def as_idle(value):
    """宽松判断：手册未说明 idle 字段的类型，避免硬断言"""
    if isinstance(value, bool):
        return value
    if isinstance(value, (int, float)):
        return value != 0
    if isinstance(value, str):
        return value in IDLE_TRUE
    return None

class SafetyMonitor(Node):
    """唯一的安全真相来源。所有发命令的地方都调用 allow_motion()。"""

    def __init__(self):
        super().__init__('safety_monitor')
        self._lock = threading.Lock()
        self._estop = False          # 锁存
        self._shutdown = False       # 锁存
        self._estop_seen = False
        self._status = None
        self._idle_raw = None

        # ① 安全底线：初始化时就订阅（手册 1.2 节第 2 条）
        self.create_subscription(
            Bool, '/fd_robot/emergency_signal', self._on_estop, 10)
        self.create_subscription(
            Bool, '/fd_robot/shutdown_signal', self._on_shutdown, 10)
        # ② 状态观察
        self.create_subscription(
            String, '/fd_robot/robot_status', self._on_status, 10)
        self.create_subscription(
            String, '/fd_robot/robot_idle_status', self._on_idle, 10)

    # ---------- 回调 ----------
    def _on_estop(self, msg):
        with self._lock:
            self._estop_seen = True
            if msg.data:
                if not self._estop:
                    self.get_logger().error('急停触发：停止发送任何运动命令')
                self._estop = True          # 只置位，不自动清除
            else:
                self.get_logger().warn('emergency_signal=false（不自动解除锁存）')

    def _on_shutdown(self, msg):
        with self._lock:
            if msg.data:
                if not self._shutdown:
                    self.get_logger().error('收到关机请求：停止业务并保存状态')
                self._shutdown = True
            else:
                self.get_logger().warn('shutdown_signal=false')

    def _on_status(self, msg):
        try:
            self._status = json.loads(msg.data)
        except json.JSONDecodeError as e:
            self.get_logger().warn(f'robot_status 解析失败: {e}')

    def _on_idle(self, msg):
        try:
            self._idle_raw = json.loads(msg.data)
        except json.JSONDecodeError:
            self._idle_raw = None

    # ---------- 查询接口 ----------
    def allow_motion(self):
        """发命令前的统一闸门。返回 (是否允许, 原因)"""
        with self._lock:
            if self._estop:
                return False, '急停锁存中'
            if self._shutdown:
                return False, '已收到关机请求'
            if not self._estop_seen:
                return False, '尚未收到急停信号，谨慎起见拒绝发送'
            return True, 'ok'

    def estop(self):
        with self._lock:
            return self._estop

    def reset_estop(self, confirmed=False):
        """仅人工确认后调用；程序中绝不自动调用"""
        if not confirmed:
            return
        with self._lock:
            self._estop = False
            self.get_logger().warn('急停锁存已人工复位')

    def idle(self):
        data = self._idle_raw
        raw = data.get('idle') if isinstance(data, dict) else None
        return as_idle(raw)

class MotionClient(Node):
    """任何会动的东西都通过这里发命令"""

    def __init__(self, safety):
        super().__init__('motion_client')
        self.safety = safety
        self.pub = self.create_publisher(
            Bool, '/demo/motion_cmd', 10)   # 仅示意，实际换成具体外设话题

    def send(self):
        ok, why = self.safety.allow_motion()
        if not ok:
            self.get_logger().error(f'拒绝发送运动命令：{why}')
            return False
        self.pub.publish(Bool(data=True))
        self.get_logger().info('已发送运动命令')
        return True

def main():
    rclpy.init()
    safety = SafetyMonitor()
    client = MotionClient(safety)

    # 多线程执行器：保证 2 Hz 的 idle 与急停回调不会被业务阻塞
    ex = MultiThreadedExecutor(num_threads=2)
    ex.add_node(safety)
    ex.add_node(client)
    threading.Thread(target=ex.spin, daemon=True).start()

    import time
    time.sleep(1.0)                       # 先把安全状态拿到手
    client.send()

    # 演示：观察 5 秒状态
    for _ in range(5):
        time.sleep(1.0)
        node = safety
        node.get_logger().info(
            f'estop={node.estop()} idle={node.idle()} '
            f'allow={node.allow_motion()[0]}')

    ex.shutdown()
    safety.destroy_node()
    client.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

7.4 C++ 端到端示例

#include <atomic>
#include <memory>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/string.hpp"

class SafetyMonitor : public rclcpp::Node {
public:
  SafetyMonitor() : Node("safety_monitor") {
    // 手册 1.2 节第 2 条：运行期间始终订阅急停
    estop_sub_ = create_subscription<std_msgs::msg::Bool>(
        "/fd_robot/emergency_signal", 10,
        [this](std_msgs::msg::Bool::SharedPtr m) {
          estop_seen_ = true;
          if (m->data) {
            if (!estop_.exchange(true))
              RCLCPP_ERROR(get_logger(), "急停触发：停止发送任何运动命令");
          } else {
            RCLCPP_WARN(get_logger(),
                        "emergency_signal=false（不自动解除锁存）");
          }
        });
    shutdown_sub_ = create_subscription<std_msgs::msg::Bool>(
        "/fd_robot/shutdown_signal", 10,
        [this](std_msgs::msg::Bool::SharedPtr m) {
          if (m->data) {
            if (!shutdown_.exchange(true))
              RCLCPP_ERROR(get_logger(), "收到关机请求：停止业务并保存状态");
          } else {
            RCLCPP_WARN(get_logger(), "shutdown_signal=false");
          }
        });
    idle_sub_ = create_subscription<std_msgs::msg::String>(
        "/fd_robot/robot_idle_status", 10,
        [this](std_msgs::msg::String::SharedPtr m) { idle_raw_ = m->data; });
  }

  /// 发命令前的统一闸门
  bool allow_motion(std::string* why = nullptr) const {
    if (estop_.load()) {
      if (why) *why = "急停锁存中";
      return false;
    }
    if (shutdown_.load()) {
      if (why) *why = "已收到关机请求";
      return false;
    }
    if (!estop_seen_.load()) {
      if (why) *why = "尚未收到急停信号，谨慎起见拒绝发送";
      return false;
    }
    return true;
  }

  void reset_estop(bool confirmed) {
    if (!confirmed) return;
    estop_.store(false);
    RCLCPP_WARN(get_logger(), "急停锁存已人工复位");
  }

  /// 手册未公开 idle 字段类型，这里做宽松匹配，不硬断言 bool
  bool idle_from_topic() const {
    return idle_raw_.find("true") != std::string::npos ||
           idle_raw_.find("idle") != std::string::npos;
  }

private:
  std::atomic<bool> estop_{false}, shutdown_{false}, estop_seen_{false};
  std::string idle_raw_;
  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_sub_, shutdown_sub_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr idle_sub_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto safety = std::make_shared<SafetyMonitor>();

  // 先把安全状态拿到手
  auto warmup = safety->now() + std::chrono::milliseconds(1000);
  while (rclcpp::ok() && safety->now() < warmup) rclcpp::spin_some(safety);

  std::string why;
  if (safety->allow_motion(&why)) {
    RCLCPP_INFO(safety->get_logger(), "允许发送运动命令");
  } else {
    RCLCPP_ERROR(safety->get_logger(), "拒绝发送运动命令：%s", why.c_str());
  }

  RCLCPP_INFO(safety->get_logger(), "idle=%s",
              safety->idle_from_topic() ? "true" : "false");

  rclcpp::spin(safety);            // 常驻，保证安全回调随时可达
  rclcpp::shutdown();
  return 0;
}

7.5 开机自检流程

7.6 异常处理决策表

八、注意事项与已知限制

8.1 附录中与本章相关的已知限制

下表摘自手册附录「已知限制」，只保留与整机状态、安全、自检、遥操作有关的条目。接入程序应按「建议用法」一列实现，不应依赖表中未说明的行为。

8.2 未收录的接口

手册第十一章开头原文：「参数库、License、固件升级、SQLite 和硬件诊断的细节不在本章范围内。」附录「未收录的接口」进一步列出：

License 没有独立查询接口，但可以从调用行为推断：调用 /fd_robot/teleoperation/control 时若 status=-1 且 message 为「遥操功能未授权」，即说明该功能未授权。手册未提供查询 License 状态的接口，不应查找 /license 下的路径。

8.3 手册未明确项

以下各点是实现过程中会遇到、但手册没有说明的内容。本文档统一标注为未明确，不做补充推断，建议在上线前向厂商确认。

8.4 排错速查

8.5 通用提醒

安全信号属于安全相关要求，不属于可选功能。急停订阅应写入第一版代码，不宜等功能完成后再补。手册 1.2 节将其排在接入步骤第 2 条，位置早于机械臂、底盘、末端执行器。

发送命令前先用 ros2 topic list、ros2 service list 确认接口存在；节点未启动时相关接口不会有任何响应（手册 1.1 节）。

第六章列出的 9 条内部接口（main_board_status、power_board/status、/diagnostics、Pico 三条、trigger_sync，以及两条重启 / 服务控制信号）仅供排查问题参考，不应与二次开发接口同时调用。

重启、关机信号不作为控制手段。手册第十一章开头与 1.1 节各说明了一次：reboot_service_signal 与 service_control_signal 用于实现运维能力，不属于业务 API。

fd_api 与 ROS 2 接口不应混用。fd_api 是另一套独立封装，与 ROS 2 接口属于不同层级，同时使用会抢占控制权（手册 1.2 节第 7 条）。