一、概述

头颈是机器人的双轴云台机构：lower 为偏航（下关节，左右转头），upper 为俯仰（上关节，上下点头）。本文覆盖手册第四章头颈的全部二次开发接口，共 9 个：6 个话题（5 个控制 + 1 个状态读取）+ 3 个服务。每个接口都配有一段可直接复制的示例（命令行 / Python / C++）。

头颈的复杂度不在接口数量，而在三件事：① 同一套接口在三种头颈电机上行为不同；② 弧度和度在同一份代码里混用；③ 偏航/俯仰与 up/down 的命名不是直觉对应。本文第二章把这些约定集中讲清，后面各接口不再重复。

1.1 与 fd_api 头部关节接口的关系

工作区已有 《fd_api 头部关节说明》（C++ 封装层）。两者是两个层级，不要混用：

1.2 核心特性

1.3 三种头颈电机类型

同一份对接代码在不同电机上行为不同。写代码前先确认机型属于哪一类：

二、运行前提与约定

2.1 环境与节点前提

# 连通性自检：能列出话题即说明域 ID 与环境正常
ros2 topic list | grep head

# 确认接口类型与字段（以本机生成的接口定义为准）
ros2 interface show sensor_msgs/msg/JointState
ros2 interface show fd_msgs/msg/HeadScurveCmd
ros2 interface show fd_msgs/srv/HeadRotate

2.2 关节名与轴向

2.3 单位约定（最容易出错的地方）

2.4 头颈软限位（单位：度）

内置默认值如下，可通过 robot_profile.yaml 中的 head_upper_range、head_lower_range 修改。超出限位的目标角度会被规划器限幅（截断后继续执行，通常无报错）：

2.5 运动方式与速度语义差异

2.6 两种写法怎么选

三、接口总览

3.1 二次开发接口（9 个）

以下为手册第四章中分割线以上的接口，是客户对接应当使用的全部头颈接口。

话题（6 个）

服务（3 个，仅 hh_servo）

3.2 系统内部接口（二次开发请勿使用）

下列接口在手册中列于「—— 系统内部 ——」分割线以下，属于驱动调试 / 自检 / 底层电机控制，不要与二次开发并行抢流：

四、话题接口详解

6 个话题按用途分两组：5 个命令话题（方向=发布）与 1 个状态话题（方向=订阅）。每个接口下方都给出「命令行 / Python / C++」三段示例。

4.1 /fd_robot/head_motor/cmd —— 双轴关节位置命令

示例 ① 命令行（ros2 CLI）

# 双轴同时下发：偏航 0 rad、俯仰 0.17 rad（约 10°）
ros2 topic pub -1 /fd_robot/head_motor/cmd sensor_msgs/msg/JointState \
  "{name: ['lower', 'upper'], position: [0.0, 0.17]}"

# 偏航转到 30°（约 0.524 rad）、俯仰归零
ros2 topic pub -1 /fd_robot/head_motor/cmd sensor_msgs/msg/JointState \
  "{name: ['lower', 'upper'], position: [0.524, 0.0]}"

# HH 舵机：发送 position≈0 使某轴初始化回中
ros2 topic pub -1 /fd_robot/head_motor/cmd sensor_msgs/msg/JointState \
  "{name: ['lower', 'upper'], position: [0.0, 0.0]}"

示例 ② Python（rclpy）

import math
import rclpy
from sensor_msgs.msg import JointState

rclpy.init()
node = rclpy.create_node('head_cmd_demo')
pub = node.create_publisher(JointState, '/fd_robot/head_motor/cmd', 10)

msg = JointState()
msg.name = ['lower', 'upper']
# 业务层用「度」，发送前统一换算成弧度
yaw_deg, pitch_deg = 0.0, 10.0
msg.position = [math.radians(yaw_deg), math.radians(pitch_deg)]

pub.publish(msg)
node.get_logger().info(f'head_motor/cmd 已下发: {msg.position}')
node.spin_once(timeout_sec=0.2)

node.destroy_node()
rclpy.shutdown()

示例 ③ C++（rclcpp）

#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.hpp>

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("head_cmd_demo");
  auto pub = node->create_publisher<sensor_msgs::msg::JointState>(
      "/fd_robot/head_motor/cmd", 10);

  sensor_msgs::msg::JointState msg;
  msg.name = {"lower", "upper"};
  const double yaw_deg = 0.0, pitch_deg = 10.0;
  msg.position = {yaw_deg * M_PI / 180.0, pitch_deg * M_PI / 180.0};

  pub->publish(msg);
  RCLCPP_INFO(node->get_logger(), "head_motor/cmd 已下发");
  rclcpp::spin_some(node);
  rclcpp::shutdown();
  return 0;
}

4.2 /fd_robot/head_up_move —— 俯仰轴目标角度

示例 ① 命令行（ros2 CLI）

# 俯仰轴转到 10°
ros2 topic pub -1 /fd_robot/head_up_move std_msgs/msg/Float32 "{data: 10.0}"

# 俯仰轴转到 -5°
ros2 topic pub -1 /fd_robot/head_up_move std_msgs/msg/Float32 "{data: -5.0}"

示例 ② Python（rclpy）

import rclpy
from std_msgs.msg import Float32

rclpy.init()
node = rclpy.create_node('head_up_demo')
pub = node.create_publisher(Float32, '/fd_robot/head_up_move', 10)

pitch_deg = 10.0                      # 单位：度
pub.publish(Float32(data=pitch_deg))
node.get_logger().info(f'head_up_move(俯仰) -> {pitch_deg} deg')
node.spin_once(timeout_sec=0.2)

node.destroy_node()
rclpy.shutdown()

示例 ③ C++（rclcpp）

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float32.hpp>

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("head_up_demo");
  auto pub = node->create_publisher<std_msgs::msg::Float32>(
      "/fd_robot/head_up_move", 10);

  std_msgs::msg::Float32 msg;
  msg.data = 10.0f;                   // 单位：度
  pub->publish(msg);
  RCLCPP_INFO(node->get_logger(), "head_up_move(俯仰) -> %.1f deg", msg.data);
  rclcpp::spin_some(node);
  rclcpp::shutdown();
  return 0;
}

4.3 /fd_robot/head_down_move —— 偏航轴目标角度

示例 ① 命令行（ros2 CLI）

# 偏航轴转到 -15°（向左转）
ros2 topic pub -1 /fd_robot/head_down_move std_msgs/msg/Float32 "{data: -15.0}"

# 偏航轴回到 0°（正前方）
ros2 topic pub -1 /fd_robot/head_down_move std_msgs/msg/Float32 "{data: 0.0}"

示例 ② Python（rclpy）

import rclpy
from std_msgs.msg import Float32

rclpy.init()
node = rclpy.create_node('head_down_demo')
pub = node.create_publisher(Float32, '/fd_robot/head_down_move', 10)

yaw_deg = -15.0                       # 单位：度，负值向左
pub.publish(Float32(data=yaw_deg))
node.get_logger().info(f'head_down_move(偏航) -> {yaw_deg} deg')
node.spin_once(timeout_sec=0.2)

node.destroy_node()
rclpy.shutdown()

示例 ③ C++（rclcpp）

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float32.hpp>

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("head_down_demo");
  auto pub = node->create_publisher<std_msgs::msg::Float32>(
      "/fd_robot/head_down_move", 10);

  std_msgs::msg::Float32 msg;
  msg.data = -15.0f;                  // 单位：度，负值向左
  pub->publish(msg);
  RCLCPP_INFO(node->get_logger(), "head_down_move(偏航) -> %.1f deg", msg.data);
  rclcpp::spin_some(node);
  rclcpp::shutdown();
  return 0;
}

4.4 /fd_robot/head_up_move_ex —— 俯仰轴目标角度 + 速度

示例 ① 命令行（ros2 CLI）

# 俯仰轴转到 10°，speed=5
ros2 topic pub -1 /fd_robot/head_up_move_ex fd_msgs/msg/HeadScurveCmd \
  "{angle: 10.0, speed: 5.0}"

# 慢速（陡度/角速度 = 2）
ros2 topic pub -1 /fd_robot/head_up_move_ex fd_msgs/msg/HeadScurveCmd \
  "{angle: 10.0, speed: 2.0}"

# 非法 speed（0）会被按默认 5 处理
ros2 topic pub -1 /fd_robot/head_up_move_ex fd_msgs/msg/HeadScurveCmd \
  "{angle: 10.0, speed: 0.0}"

示例 ② Python（rclpy）

import rclpy
from fd_msgs.msg import HeadScurveCmd

rclpy.init()
node = rclpy.create_node('head_up_ex_demo')
pub = node.create_publisher(HeadScurveCmd, '/fd_robot/head_up_move_ex', 10)

msg = HeadScurveCmd()
msg.angle = 10.0          # 度
msg.speed = 5.0           # damiao/gm6020: 陡度；hh_servo: deg/s

pub.publish(msg)
node.get_logger().info(f'up_ex: angle={msg.angle} speed={msg.speed}')
node.spin_once(timeout_sec=0.2)

node.destroy_node()
rclpy.shutdown()

示例 ③ C++（rclcpp）

#include <rclcpp/rclcpp.hpp>
#include <fd_msgs/msg/head_scurve_cmd.hpp>

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("head_up_ex_demo");
  auto pub = node->create_publisher<fd_msgs::msg::HeadScurveCmd>(
      "/fd_robot/head_up_move_ex", 10);

  fd_msgs::msg::HeadScurveCmd msg;
  msg.angle = 10.0f;      // 度
  msg.speed = 5.0f;       // damiao/gm6020: 陡度；hh_servo: deg/s
  pub->publish(msg);
  RCLCPP_INFO(node->get_logger(), "up_ex angle=%.1f speed=%.1f",
              msg.angle, msg.speed);
  rclcpp::spin_some(node);
  rclcpp::shutdown();
  return 0;
}

4.5 /fd_robot/head_down_move_ex —— 偏航轴目标角度 + 速度

示例 ① 命令行（ros2 CLI）

# 偏航轴转到 -15°，speed=5
ros2 topic pub -1 /fd_robot/head_down_move_ex fd_msgs/msg/HeadScurveCmd \
  "{angle: -15.0, speed: 5.0}"

示例 ② Python（rclpy）

import rclpy
from fd_msgs.msg import HeadScurveCmd

rclpy.init()
node = rclpy.create_node('head_down_ex_demo')
pub = node.create_publisher(HeadScurveCmd, '/fd_robot/head_down_move_ex', 10)

msg = HeadScurveCmd()
msg.angle = -15.0         # 度，负值向左
msg.speed = 5.0
pub.publish(msg)
node.get_logger().info(f'down_ex: angle={msg.angle} speed={msg.speed}')
node.spin_once(timeout_sec=0.2)

node.destroy_node()
rclpy.shutdown()

示例 ③ C++（rclcpp）

#include <rclcpp/rclcpp.hpp>
#include <fd_msgs/msg/head_scurve_cmd.hpp>

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("head_down_ex_demo");
  auto pub = node->create_publisher<fd_msgs::msg::HeadScurveCmd>(
      "/fd_robot/head_down_move_ex", 10);

  fd_msgs::msg::HeadScurveCmd msg;
  msg.angle = -15.0f;     // 度，负值向左
  msg.speed = 5.0f;
  pub->publish(msg);
  RCLCPP_INFO(node->get_logger(), "down_ex angle=%.1f speed=%.1f",
              msg.angle, msg.speed);
  rclcpp::spin_some(node);
  rclcpp::shutdown();
  return 0;
}

4.6 /fd_robot/head/current_pose —— 当前头颈姿态

示例 ① 命令行（ros2 CLI）

# 实时查看当前姿态（弧度）
ros2 topic echo /fd_robot/head/current_pose

# 只看前 3 条
ros2 topic echo /fd_robot/head/current_pose --once

示例 ② Python（rclpy）

import math
import rclpy
from sensor_msgs.msg import JointState

rclpy.init()
node = rclpy.create_node('head_pose_demo')

def on_pose(msg: JointState):
    # 顺序固定：lower=偏航, upper=俯仰
    yaw = msg.position[0]
    pitch = msg.position[1]
    if math.isnan(yaw) or math.isnan(pitch):
        node.get_logger().warn('姿态为 NaN —— HH 舵机请先调用 initialize')
        return
    node.get_logger().info(
        f'偏航 {math.degrees(yaw):.1f}° / 俯仰 {math.degrees(pitch):.1f}°')

node.create_subscription(
    JointState, '/fd_robot/head/current_pose', on_pose, 10)

rclpy.spin(node)

示例 ③ C++（rclcpp）

#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.hpp>

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

  auto sub = node->create_subscription<sensor_msgs::msg::JointState>(
      "/fd_robot/head/current_pose", 10,
      [node](const sensor_msgs::msg::JointState::SharedPtr msg) {
        if (msg->position.size() < 2) return;
        const double yaw = msg->position[0], pitch = msg->position[1];
        if (std::isnan(yaw) || std::isnan(pitch)) {
          RCLCPP_WARN(node->get_logger(),
                      "姿态为 NaN —— HH 舵机请先调用 initialize");
          return;
        }
        RCLCPP_INFO(node->get_logger(),
                    "偏航 %.1f deg / 俯仰 %.1f deg",
                    yaw * 180.0 / M_PI, pitch * 180.0 / M_PI);
      });

  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

五、服务接口详解

3 个服务全部仅在 head_yaw_motor=hh_servo 时存在。使用 damiao / gm6020 的头颈上，这些服务不会被注册，调用会直接失败（找不到服务）。

5.1 /fd_robot/head/rotate —— 转动到指定舵机位置

示例 ① 命令行（ros2 CLI）

# 偏航轴（servo_id=1）转到中位
ros2 service call /fd_robot/head/rotate fd_msgs/srv/HeadRotate \
  "{servo_id: 1, angle: 500}"

# 俯仰轴（servo_id=2）给一个偏上的刻度
ros2 service call /fd_robot/head/rotate fd_msgs/srv/HeadRotate \
  "{servo_id: 2, angle: 600}"

# 查看接口定义，确认字段名
ros2 interface show fd_msgs/srv/HeadRotate

示例 ② Python（rclpy）

import rclpy
from fd_msgs.srv import HeadRotate

rclpy.init()
node = rclpy.create_node('head_rotate_demo')

client = node.create_client(HeadRotate, '/fd_robot/head/rotate')
if not client.wait_for_service(timeout_sec=2.0):
    node.get_logger().error('服务不可用 —— 请确认 head_yaw_motor=hh_servo')
    raise SystemExit(1)

req = HeadRotate.Request()
req.servo_id = 1        # 1=偏航, 2=俯仰
req.angle = 500         # 舵机刻度 0-1000，中位约 500

future = client.call_async(req)
rclpy.spin_until_future_complete(node, future)
res = future.result()
node.get_logger().info(f'success={res.success} message={res.message}')

node.destroy_node()
rclpy.shutdown()

示例 ③ C++（rclcpp）

#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <fd_msgs/srv/head_rotate.hpp>

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("head_rotate_demo");
  auto client = node->create_client<fd_msgs::srv::HeadRotate>(
      "/fd_robot/head/rotate");

  if (!client->wait_for_service(std::chrono::seconds(2))) {
    RCLCPP_ERROR(node->get_logger(),
                 "服务不可用 —— 请确认 head_yaw_motor=hh_servo");
    return 1;
  }

  auto req = std::make_shared<fd_msgs::srv::HeadRotate::Request>();
  req->servo_id = 1;    // 1=偏航, 2=俯仰
  req->angle = 500;     // 舵机刻度 0-1000

  auto future = client->async_send_request(req);
  if (rclcpp::spin_until_future_complete(node, future) ==
      rclcpp::FutureReturnCode::SUCCESS) {
    auto res = future.get();
    RCLCPP_INFO(node->get_logger(), "success=%d message=%s",
                res->success, res->message.c_str());
  }
  rclcpp::shutdown();
  return 0;
}

5.2 /fd_robot/head/initialize —— 初始化复位

示例 ① 命令行（ros2 CLI）

# 初始化双轴（servo_ids 为空 = 默认 1、2）
ros2 service call /fd_robot/head/initialize fd_msgs/srv/HeadInitialize "{servo_ids: []}"

# 只初始化偏航轴
ros2 service call /fd_robot/head/initialize fd_msgs/srv/HeadInitialize "{servo_ids: [1]}"

# 初始化双轴（显式写法）
ros2 service call /fd_robot/head/initialize fd_msgs/srv/HeadInitialize "{servo_ids: [1, 2]}"

示例 ② Python（rclpy）

import rclpy
from fd_msgs.srv import HeadInitialize

rclpy.init()
node = rclpy.create_node('head_init_demo')

client = node.create_client(HeadInitialize, '/fd_robot/head/initialize')
if not client.wait_for_service(timeout_sec=2.0):
    node.get_logger().error('服务不可用 —— 请确认 head_yaw_motor=hh_servo')
    raise SystemExit(1)

req = HeadInitialize.Request()
req.servo_ids = []          # 空 = 默认初始化 1、2

future = client.call_async(req)
rclpy.spin_until_future_complete(node, future)
node.get_logger().info(f'initialize 完成: {future.result()}')

node.destroy_node()
rclpy.shutdown()

示例 ③ C++（rclcpp）

#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include <fd_msgs/srv/head_initialize.hpp>

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("head_init_demo");
  auto client = node->create_client<fd_msgs::srv::HeadInitialize>(
      "/fd_robot/head/initialize");

  if (!client->wait_for_service(std::chrono::seconds(2))) {
    RCLCPP_ERROR(node->get_logger(), "服务不可用");
    return 1;
  }

  auto req = std::make_shared<fd_msgs::srv::HeadInitialize::Request>();
  req->servo_ids = {};      // 空 = 默认初始化 1、2

  auto future = client->async_send_request(req);
  if (rclcpp::spin_until_future_complete(node, future) ==
      rclcpp::FutureReturnCode::SUCCESS) {
    RCLCPP_INFO(node->get_logger(), "initialize 完成");
  }
  rclcpp::shutdown();
  return 0;
}

5.3 /fd_robot/head/is_online —— 查询舵机是否在线

示例 ① 命令行（ros2 CLI）

ros2 service call /fd_robot/head/is_online fd_msgs/srv/HeadIsOnline "{}"

示例 ② Python（rclpy）

import rclpy
from fd_msgs.srv import HeadIsOnline

rclpy.init()
node = rclpy.create_node('head_online_demo')

client = node.create_client(HeadIsOnline, '/fd_robot/head/is_online')
if not client.wait_for_service(timeout_sec=2.0):
    node.get_logger().error('服务不可用 —— 请确认 head_yaw_motor=hh_servo')
    raise SystemExit(1)

future = client.call_async(HeadIsOnline.Request())
rclpy.spin_until_future_complete(node, future)

res = future.result()
if res.online:
    node.get_logger().info(f'舵机在线: {res.message}')
else:
    node.get_logger().error(f'舵机离线: {res.message}')

node.destroy_node()
rclpy.shutdown()

示例 ③ C++（rclcpp）

#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include <fd_msgs/srv/head_is_online.hpp>

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("head_online_demo");
  auto client = node->create_client<fd_msgs::srv::HeadIsOnline>(
      "/fd_robot/head/is_online");

  if (!client->wait_for_service(std::chrono::seconds(2))) {
    RCLCPP_ERROR(node->get_logger(), "服务不可用");
    return 1;
  }

  auto future = client->async_send_request(
      std::make_shared<fd_msgs::srv::HeadIsOnline::Request>());
  if (rclcpp::spin_until_future_complete(node, future) ==
      rclcpp::FutureReturnCode::SUCCESS) {
    auto res = future.get();
    if (res->online) {
      RCLCPP_INFO(node->get_logger(), "舵机在线: %s", res->message.c_str());
    } else {
      RCLCPP_ERROR(node->get_logger(), "舵机离线: %s", res->message.c_str());
    }
  }
  rclcpp::shutdown();
  return 0;
}

六、典型流程与完整示例

6.1 对接步骤

先按 1.3 判断你的头颈属于哪一类，再走对应分支：

6.2 完整闭环示例（Python）

下面这段程序走完 自检 → 初始化 → 读初值 → 运动 → 到位判定 全流程。开头通过一次 is_online 探测自动分流 hh_servo 与其它类型：

import math
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import Float32
from fd_msgs.srv import HeadIsOnline, HeadInitialize


class HeadController(Node):
    def __init__(self):
        super().__init__('head_controller')
        self.latest = None
        self.create_subscription(
            JointState, '/fd_robot/head/current_pose', self._on_pose, 10)

        self.pub_up = self.create_publisher(
            Float32, '/fd_robot/head_up_move', 10)
        self.pub_down = self.create_publisher(
            Float32, '/fd_robot/head_down_move', 10)

        self.cli_online = self.create_client(
            HeadIsOnline, '/fd_robot/head/is_online')
        self.cli_init = self.create_client(
            HeadInitialize, '/fd_robot/head/initialize')

    def _on_pose(self, msg: JointState):
        self.latest = msg

    def _call(self, client, req):
        if not client.wait_for_service(timeout_sec=2.0):
            return None
        fut = client.call_async(req)
        rclpy.spin_until_future_complete(self, fut)
        return fut.result()

    def setup(self):
        """hh_servo 分支：在线检查 + 初始化；其它类型直接跳过。"""
        res = self._call(self.cli_online, HeadIsOnline.Request())
        if res is None:
            self.get_logger().info('无 is_online 服务 -> 认定为 damiao / gm6020')
            return
        if not res.online:
            raise RuntimeError(f'舵机离线: {res.message}')
        req = HeadInitialize.Request()
        req.servo_ids = []          # 空 = 默认 1、2
        self._call(self.cli_init, req)
        self.get_logger().info('hh_servo 已初始化')

    def read_pose(self, timeout=2.0):
        """读取姿态，返回 (偏航deg, 俯仰deg)；NaN 视为无效。"""
        deadline = self.get_clock().now().nanoseconds + int(timeout * 1e9)
        while rclpy.ok():
            rclpy.spin_once(self, timeout_sec=0.1)
            if self.latest is not None and len(self.latest.position) >= 2:
                yaw_r, pitch_r = self.latest.position[0], self.latest.position[1]
                if not (math.isnan(yaw_r) or math.isnan(pitch_r)):
                    return math.degrees(yaw_r), math.degrees(pitch_r)
            if self.get_clock().now().nanoseconds > deadline:
                raise TimeoutError('等待 current_pose 超时')
        raise RuntimeError('rclpy 已关闭')

    def move(self, yaw_deg, pitch_deg, tol_deg=2.0, timeout=5.0):
        """下发双轴目标并等待到位。"""
        self.pub_down.publish(Float32(data=float(yaw_deg)))
        self.pub_up.publish(Float32(data=float(pitch_deg)))
        self.get_logger().info(f'目标: 偏航 {yaw_deg} / 俯仰 {pitch_deg}')

        deadline = self.get_clock().now().nanoseconds + int(timeout * 1e9)
        while rclpy.ok():
            y, p = self.read_pose()
            if abs(y - yaw_deg) <= tol_deg and abs(p - pitch_deg) <= tol_deg:
                self.get_logger().info(f'到位: 偏航 {y:.1f} / 俯仰 {p:.1f}')
                return True
            if self.get_clock().now().nanoseconds > deadline:
                self.get_logger().warn(
                    f'超时未到位: 当前 偏航 {y:.1f} / 俯仰 {p:.1f}')
                return False
        return False


def main():
    rclpy.init()
    ctrl = HeadController()
    try:
        ctrl.setup()
        ctrl.read_pose()
        ctrl.move(yaw_deg=0.0, pitch_deg=10.0)     # 抬头看前方
        ctrl.move(yaw_deg=-15.0, pitch_deg=0.0)    # 左转回平
    finally:
        ctrl.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

6.3 到位判定怎么写

七、注意事项与已知限制

7.1 硬性约束

7.2 已知限制（手册附录）

7.3 排查速查