一、概述

升降是机器人的垂直高度调节机构（整机 Z 向升降立柱）。它接口很少，但有一个必须先分清的前提：手册里的升降有两条完全不同的实现路径，走错一条，一个接口都调不通。

本文覆盖手册第六章的全部二次开发接口 6 条（5 个命令发布话题 + 1 个状态订阅话题），以及第二章列出的 2 条系统内部接口，用于排查问题。

对接升降真正要小心的是三件事：① 单位是米，不是毫米（发 100 会被限幅到 0.7）；② 位置运动前必须先设速度，否则位置命令没有速度依据；③ 到位判定靠读 current_pose，不是靠固定延时。

1.1 与 fd_api 升降接口的关系

工作区已有 《fd_api 升降机构说明》（C++ 封装层）。两者是两个层级，不要混用：

1.2 升降在整机中的位置

升降没有应用层服务，也没有动作接口——它是最"薄"的一层：程序直接向节点发布话题，节点转发给 Modbus 驱动。因此本章的全部接口都是话题。

二、运行前提与约定

2.1 环境与节点前提

2.2 如何确认接口可用

# 1) 确认能列出升降话题
ros2 topic list | grep lifting_motor

# 期望看到（全部 6 条）
# /fd_robot/lifting_motor/current_pose
# /fd_robot/lifting_motor/home_cmd
# /fd_robot/lifting_motor/position_cmd
# /fd_robot/lifting_motor/position_set_speed_cmd
# /fd_robot/lifting_motor/status
# /fd_robot/lifting_motor/velocity_cmd

# 2) 确认状态反馈在发布（20 Hz）
ros2 topic hz /fd_robot/lifting_motor/current_pose

# 3) 确认消息类型
ros2 topic info /fd_robot/lifting_motor/position_cmd -v

2.3 单位与取值约定

2.4 行程与限幅

目标高度会被限幅到 [0, 0.7] m。注意这是限幅（静默截断）而不是拒绝：超出范围的值被截断到边界后继续执行，调用方通常不会收到错误提示。

速度方面，手册 6.4 节给出：yako 的速度上限约为 ±0.2 m/s；执行位置运动前，请先通过 position_set_speed_cmd 设置速度（单位 m/s，yako 同样以 0.2 m/s 为上限）。

2.5 如何判断「到位」

手册 6.4 节末尾只有一句：「请通过 current_pose 判断是否到位。」升降没有 Action，也没有运动状态话题，所以没有 busy、result_code、REACHED 这类标志可用。

推荐写法是「读位置 → 与目标比较容差」：

# 目标 0.10 m，当前高度读回来接近 0.10 就认为到位
ros2 topic echo --once /fd_robot/lifting_motor/current_pose

2.6 安全前提

三、接口总览

3.1 二次开发接口（6 条）

方向从二次开发程序的角度理解：「发布」表示由你的程序发送命令，「订阅」表示由你的程序接收反馈。

3.2 系统内部接口（2 条）

下列接口由驱动和诊断底层使用，手册在 6.1 / 6.2 节把它们标为「系统内部接口」。列出仅供排查问题时参考，请勿与二次开发接口同时调用。

3.3 关于动作接口

四、话题接口详解

本章按手册 6.1 节的顺序，逐个说明 6 个二次开发话题：先 5 个命令发布，再 1 个状态订阅。每个接口固定给出「语义卡片 → 字段表 → 三段示例（命令行 / Python / C++）」。

示例

① 命令行（ros2 CLI）

# ① 先设速度（顺序不能反）
ros2 topic pub -1 /fd_robot/lifting_motor/position_set_speed_cmd \
  std_msgs/msg/Float32 "{data: 0.05}"

# ② 再发目标高度：0.10 m（= 100 mm）
ros2 topic pub -1 /fd_robot/lifting_motor/position_cmd \
  std_msgs/msg/Float32 "{data: 0.10}"

# ③ 回读当前高度确认
ros2 topic echo --once /fd_robot/lifting_motor/current_pose

# 查看消息定义
ros2 interface show std_msgs/msg/Float32

② Python（rclpy）

# 示意代码：先设速度、再发位置、最后读 current_pose 判到位
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32

TOL_M = 0.005          # 工程建议容差 5 mm，手册未给该值，请按机型调整

class LiftPosition(Node):
    def __init__(self):
        super().__init__('lift_position')
        self.pub_speed = self.create_publisher(
            Float32, '/fd_robot/lifting_motor/position_set_speed_cmd', 10)
        self.pub_pos = self.create_publisher(
            Float32, '/fd_robot/lifting_motor/position_cmd', 10)
        self.sub_pos = self.create_subscription(
            Float32, '/fd_robot/lifting_motor/current_pose', self.on_pose, 10)
        self.cur = None

    def on_pose(self, msg):
        self.cur = msg.data

    def _pub_n(self, pub, value, n=3):
        # 重复发布几次，避免订阅端尚未建立连接时丢消息（工程建议）
        msg = Float32()
        msg.data = float(value)
        for _ in range(n):
            pub.publish(msg)
            rclpy.spin_once(self, timeout_sec=0.05)

    def move_to(self, height_m, speed_mps=0.05, timeout_sec=20.0):
        if not 0.0 <= height_m <= 0.7:
            self.get_logger().warn(f'目标 {height_m} m 超出行程 0–0.7，将被限幅')
        self._pub_n(self.pub_speed, speed_mps)   # ① 设速度
        self._pub_n(self.pub_pos, height_m)      # ② 发位置

        deadline = self.get_clock().now().nanoseconds + int(timeout_sec * 1e9)
        while rclpy.ok():
            rclpy.spin_once(self, timeout_sec=0.1)
            if self.cur is not None:
                self.get_logger().info(f'当前高度 {self.cur:.4f} m')
                if abs(self.cur - height_m) < TOL_M:
                    self.get_logger().info('到位')
                    return True
            if self.get_clock().now().nanoseconds > deadline:
                self.get_logger().error('超时未到位')
                return False

def main():
    rclpy.init()
    node = LiftPosition()
    node.move_to(0.10)        # 升到 0.10 m = 100 mm
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <chrono>
#include <cmath>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/float32.hpp"

using namespace std::chrono_literals;
static constexpr double TOL_M = 0.005;   // 工程建议容差，手册未给该值

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

  auto pub_speed = node->create_publisher<std_msgs::msg::Float32>(
      "/fd_robot/lifting_motor/position_set_speed_cmd", 10);
  auto pub_pos = node->create_publisher<std_msgs::msg::Float32>(
      "/fd_robot/lifting_motor/position_cmd", 10);

  double cur = std::nan("");
  auto sub_pos = node->create_subscription<std_msgs::msg::Float32>(
      "/fd_robot/lifting_motor/current_pose", 10,
      [&cur](std_msgs::msg::Float32::SharedPtr msg) { cur = msg->data; });

  // ① 先设速度（0.05 m/s）
  std_msgs::msg::Float32 s;
  s.data = 0.05f;
  for (int i = 0; i < 3; ++i) { pub_speed->publish(s); rclcpp::sleep_for(50ms); }

  // ② 再发目标高度 0.10 m（= 100 mm）
  std_msgs::msg::Float32 p;
  p.data = 0.10f;
  for (int i = 0; i < 3; ++i) { pub_pos->publish(p); rclcpp::sleep_for(50ms); }

  // ③ 读 current_pose 判断到位，不要用固定延时
  auto deadline = node->now() + 20s;
  while (rclcpp::ok() && node->now() < deadline) {
    rclcpp::spin_some(node);
    if (!std::isnan(cur)) {
      RCLCPP_INFO(node->get_logger(), "当前高度 %.4f m", cur);
      if (std::fabs(cur - 0.10) < TOL_M) {
        RCLCPP_INFO(node->get_logger(), "到位");
        break;
      }
    }
    rclcpp::sleep_for(100ms);
  }

  rclcpp::shutdown();
  return 0;
}

示例

① 命令行（ros2 CLI）

# 设为较慢的 0.03 m/s（适合首次调试、行程较小时）
ros2 topic pub -1 /fd_robot/lifting_motor/position_set_speed_cmd \
  std_msgs/msg/Float32 "{data: 0.03}"

# 之后再发位置命令即可沿用该速度
ros2 topic pub -1 /fd_robot/lifting_motor/position_cmd \
  std_msgs/msg/Float32 "{data: 0.15}"

② Python（rclpy）

# 示意代码：把"设速度"单独封装成一个方法，便于复用到每次位置运动
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32

class LiftSpeed(Node):
    def __init__(self):
        super().__init__('lift_speed')
        self.pub = self.create_publisher(
            Float32, '/fd_robot/lifting_motor/position_set_speed_cmd', 10)

    def set_speed(self, speed_mps):
        if speed_mps > 0.2:
            self.get_logger().warn(f'{speed_mps} m/s 超过 yako 约 0.2 m/s 的上限')
        msg = Float32()
        msg.data = float(speed_mps)
        for _ in range(3):
            self.pub.publish(msg)
            rclpy.spin_once(self, timeout_sec=0.05)
        self.get_logger().info(f'已设置后续位置运动速度 {speed_mps} m/s')

def main():
    rclpy.init()
    node = LiftSpeed()
    node.set_speed(0.03)      # 先用低速验证
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

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

using namespace std::chrono_literals;

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

  const double speed_mps = 0.03;      // yako 上限约 0.2 m/s
  if (speed_mps > 0.2) {
    RCLCPP_WARN(node->get_logger(), "速度超过 yako 约 0.2 m/s 的上限");
  }

  std_msgs::msg::Float32 msg;
  msg.data = static_cast<float>(speed_mps);
  for (int i = 0; i < 3; ++i) {
    pub->publish(msg);
    rclcpp::sleep_for(50ms);
  }
  RCLCPP_INFO(node->get_logger(), "已设置后续位置运动速度 %.2f m/s", speed_mps);

  rclcpp::shutdown();
  return 0;
}

示例

① 命令行（ros2 CLI）

# 正向点动 0.05 m/s（务必先确认方向再提速）
ros2 topic pub -1 /fd_robot/lifting_motor/velocity_cmd \
  std_msgs/msg/Float32 "{data: 0.05}"

# 反方向
ros2 topic pub -1 /fd_robot/lifting_motor/velocity_cmd \
  std_msgs/msg/Float32 "{data: -0.05}"

# 停止：速度归零
ros2 topic pub -1 /fd_robot/lifting_motor/velocity_cmd \
  std_msgs/msg/Float32 "{data: 0.0}"

# 或直接用字符串命令急停（见 4.5 节）
ros2 topic pub -1 /fd_robot/lifting_motor/status \
  std_msgs/msg/String "{data: 'motor_stop'}"

② Python（rclpy）

# 示意代码：定长点动 —— 按速度走指定时长后自动停止
import time
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32

class LiftJog(Node):
    def __init__(self):
        super().__init__('lift_jog')
        self.pub = self.create_publisher(
            Float32, '/fd_robot/lifting_motor/velocity_cmd', 10)

    def _pub(self, v):
        msg = Float32()
        msg.data = float(v)
        self.pub.publish(msg)

    def jog(self, speed_mps, duration_sec):
        self.get_logger().info(f'点动 {speed_mps} m/s，持续 {duration_sec} s')
        end = time.time() + duration_sec
        while time.time() < end:
            self._pub(speed_mps)      # 持续下发，避免指令丢失后停住
            rclpy.spin_once(self, timeout_sec=0.05)
        self._pub(0.0)                # 显式停止
        self.get_logger().info('已停止')

def main():
    rclpy.init()
    node = LiftJog()
    node.jog(0.05, 1.0)               # 缓升 1 秒
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

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

using namespace std::chrono_literals;

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

  std_msgs::msg::Float32 cmd;
  cmd.data = 0.05f;                       // 正向 0.05 m/s
  auto stop_at = node->now() + 1s;        // 点动 1 秒
  while (rclcpp::ok() && node->now() < stop_at) {
    pub->publish(cmd);
    rclcpp::sleep_for(50ms);
  }

  cmd.data = 0.0f;                        // 显式停止
  for (int i = 0; i < 3; ++i) { pub->publish(cmd); rclcpp::sleep_for(50ms); }
  RCLCPP_INFO(node->get_logger(), "点动结束，已停止");

  rclcpp::shutdown();
  return 0;
}

示例

① 命令行（ros2 CLI）

# 触发回原点
ros2 topic pub -1 /fd_robot/lifting_motor/home_cmd std_msgs/msg/Empty "{}"

# 回原点期间持续观察高度变化
ros2 topic echo /fd_robot/lifting_motor/current_pose

② Python（rclpy）

# 示意代码：触发回原点，并等待高度稳定在原点附近
import rclpy
from rclpy.node import Node
from std_msgs.msg import Empty, Float32

ORIGIN_TOL_M = 0.005      # 工程建议值，手册未给原点判定容差

class LiftHome(Node):
    def __init__(self):
        super().__init__('lift_home')
        self.pub = self.create_publisher(
            Empty, '/fd_robot/lifting_motor/home_cmd', 10)
        self.sub = self.create_subscription(
            Float32, '/fd_robot/lifting_motor/current_pose', self.on_pose, 10)
        self.cur = None

    def on_pose(self, msg):
        self.cur = msg.data

    def go_home(self, timeout_sec=30.0):
        self.pub.publish(Empty())        # 空消息，发布即触发
        self.get_logger().info('已触发回原点')
        deadline = self.get_clock().now().nanoseconds + int(timeout_sec * 1e9)
        while rclpy.ok():
            rclpy.spin_once(self, timeout_sec=0.1)
            if self.cur is not None and abs(self.cur) < ORIGIN_TOL_M:
                self.get_logger().info(f'已回到原点附近：{self.cur:.4f} m')
                return True
            if self.get_clock().now().nanoseconds > deadline:
                self.get_logger().error('回原点超时')
                return False

def main():
    rclpy.init()
    node = LiftHome()
    node.go_home()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <chrono>
#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/empty.hpp"
#include "std_msgs/msg/float32.hpp"

using namespace std::chrono_literals;
static constexpr double ORIGIN_TOL_M = 0.005;   // 工程建议值

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("lift_home");
  auto pub = node->create_publisher<std_msgs::msg::Empty>(
      "/fd_robot/lifting_motor/home_cmd", 10);

  double cur = -1.0;
  auto sub = node->create_subscription<std_msgs::msg::Float32>(
      "/fd_robot/lifting_motor/current_pose", 10,
      [&cur](std_msgs::msg::Float32::SharedPtr msg) { cur = msg->data; });

  pub->publish(std_msgs::msg::Empty());
  RCLCPP_INFO(node->get_logger(), "已触发回原点");

  auto deadline = node->now() + 30s;
  while (rclcpp::ok() && node->now() < deadline) {
    rclcpp::spin_some(node);
    if (cur >= 0.0 && std::fabs(cur) < ORIGIN_TOL_M) {
      RCLCPP_INFO(node->get_logger(), "已回到原点附近：%.4f m", cur);
      break;
    }
    rclcpp::sleep_for(100ms);
  }

  rclcpp::shutdown();
  return 0;
}

示例

① 命令行（ros2 CLI）

# 立即停止
ros2 topic pub -1 /fd_robot/lifting_motor/status std_msgs/msg/String "{data: 'motor_stop'}"

# 把当前位置标定为原点
ros2 topic pub -1 /fd_robot/lifting_motor/status std_msgs/msg/String "{data: 'set_oring'}"

# 复位
ros2 topic pub -1 /fd_robot/lifting_motor/status std_msgs/msg/String "{data: 'reset'}"

# 清除编码器异常
ros2 topic pub -1 /fd_robot/lifting_motor/status \
  std_msgs/msg/String "{data: 'encoder_exception_clear'}"

② Python（rclpy）

# 示意代码：把 5 个命令词封装成常量，避免拼写错误
import rclpy
from rclpy.node import Node
from std_msgs.msg import String

CMD_MOTOR_STOP = 'motor_stop'
CMD_SET_ORIGIN = 'set_oring'          # 注意是 set_oring，不是 set_origin
CMD_RESET = 'reset'
CMD_SERVO_DISABLE = 'servo_disable'
CMD_ENCODER_CLEAR = 'encoder_exception_clear'
VALID_CMDS = {CMD_MOTOR_STOP, CMD_SET_ORIGIN, CMD_RESET,
              CMD_SERVO_DISABLE, CMD_ENCODER_CLEAR}

class LiftStatusCmd(Node):
    def __init__(self):
        super().__init__('lift_status_cmd')
        self.pub = self.create_publisher(
            String, '/fd_robot/lifting_motor/status', 10)

    def send(self, cmd):
        if cmd not in VALID_CMDS:
            self.get_logger().error(f'未知命令词 {cmd}，已拒绝发送')
            return False
        msg = String()
        msg.data = cmd
        for _ in range(3):
            self.pub.publish(msg)
            rclpy.spin_once(self, timeout_sec=0.05)
        self.get_logger().info(f'已发送 {cmd}')
        return True

def main():
    rclpy.init()
    node = LiftStatusCmd()
    node.send(CMD_MOTOR_STOP)          # 立即停止
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

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

using namespace std::chrono_literals;

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("lift_status_cmd");
  auto pub = node->create_publisher<std_msgs::msg::String>(
      "/fd_robot/lifting_motor/status", 10);

  const std::string cmd = "motor_stop";   // 另有 set_oring / reset /
                                          // servo_disable / encoder_exception_clear
  std_msgs::msg::String msg;
  msg.data = cmd;
  for (int i = 0; i < 3; ++i) {
    pub->publish(msg);
    rclcpp::sleep_for(50ms);
  }
  RCLCPP_INFO(node->get_logger(), "已发送 %s", cmd.c_str());

  rclcpp::shutdown();
  return 0;
}

示例

① 命令行（ros2 CLI）

# 持续观察
ros2 topic echo /fd_robot/lifting_motor/current_pose

# 只取一次当前值
ros2 topic echo --once /fd_robot/lifting_motor/current_pose

# 确认发布频率（应为 20 Hz 左右）
ros2 topic hz /fd_robot/lifting_motor/current_pose

② Python（rclpy）

# 示意代码：订阅并做"到位判定 + 超时保护"
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32

class LiftMonitor(Node):
    def __init__(self):
        super().__init__('lift_monitor')
        self.sub = self.create_subscription(
            Float32, '/fd_robot/lifting_motor/current_pose', self.on_pose, 10)
        self.cur = None

    def on_pose(self, msg):
        self.cur = msg.data

    def wait_until(self, target_m, tol_m=0.005, timeout_sec=20.0):
        # tol_m 为工程建议值，手册未给出到位容差
        deadline = self.get_clock().now().nanoseconds + int(timeout_sec * 1e9)
        while rclpy.ok():
            rclpy.spin_once(self, timeout_sec=0.05)
            if self.cur is None:
                continue
            if abs(self.cur - target_m) < tol_m:
                self.get_logger().info(f'到位：{self.cur:.4f} m')
                return True
            if self.get_clock().now().nanoseconds > deadline:
                self.get_logger().error(f'超时，当前 {self.cur:.4f} m')
                return False

def main():
    rclpy.init()
    node = LiftMonitor()
    node.wait_until(0.10)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <chrono>
#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/float32.hpp"

using namespace std::chrono_literals;

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

  double cur = std::nan("");
  auto sub = node->create_subscription<std_msgs::msg::Float32>(
      "/fd_robot/lifting_motor/current_pose", 10,
      [&cur](std_msgs::msg::Float32::SharedPtr msg) {
        cur = msg->data;
        RCLCPP_INFO_THROTTLE(rclcpp::get_logger("lift_monitor"),
                             *rclcpp::Clock::make_shared(), 500,
                             "当前高度 %.4f m", cur);
      });

  // 阻塞等待：把 spin 放在循环里，避免订阅回调不执行
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

五、系统内部接口

手册 6.1 / 6.2 节把下面 2 条标为系统内部接口：由驱动与诊断底层使用，仅供排查问题时参考，请勿与二次开发接口同时调用。手册 2.2 节也说明：系统内部接口不逐一展开字段，如需查看可对类型名执行 ros2 interface show。

# 查看字段定义
ros2 interface show fd_msgs/msg/MotorInfoMsg

# 观察电机信息
ros2 topic echo /fd_robot/lifting_motor/motor_info

SelfCheckSrv 是各子系统自检的通用类型（手册 11.3 节给出字段）：

# 单独调用升降自检
ros2 service call /fd_robot/self_check/lifting_motor fd_msgs/srv/SelfCheckSrv "{}"

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

六、典型流程与完整示例

6.1 首次对接步骤

6.2 位置运动闭环

升降没有 Action，闭环要自己搭。标准顺序只有三步：

6.3 点动控制（velocity_cmd）

点动适合微调和示教前的粗定位：按住走、松开停。因为没有位置目标，程序需要自己做「走多久 / 走到哪停」。

6.4 回原点

# ① 触发回原点（Empty 消息）
ros2 topic pub -1 /fd_robot/lifting_motor/home_cmd std_msgs/msg/Empty "{}"

# ② 观察高度变化，确认在动、方向正确
ros2 topic echo /fd_robot/lifting_motor/current_pose

# ③ 高度稳定在原点附近即完成（容差需自行确定，手册未给）

6.5 停止 / 复位类命令对照

6.6 完整示例：Python 端到端

下面这份脚本把「前置检查 → 急停监听 → 设速度 → 发位置 → 等到位 → 停止」串成了一条完整链路，可直接作为对接起点（示意代码，请按实际机型调整容差与速度）。

# lift_demo.py —— 升降对接端到端示例（示意代码）
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool, Empty, Float32, String

TOL_M = 0.005           # 工程建议容差，手册未给出该值
TOPIC_POSE = '/fd_robot/lifting_motor/current_pose'
TOPIC_SET_SPEED = '/fd_robot/lifting_motor/position_set_speed_cmd'
TOPIC_POS = '/fd_robot/lifting_motor/position_cmd'
TOPIC_STATUS = '/fd_robot/lifting_motor/status'
TOPIC_ESTOP = '/fd_robot/emergency_signal'

class LiftClient(Node):
    def __init__(self):
        super().__init__('lift_demo')
        self.pub_speed = self.create_publisher(Float32, TOPIC_SET_SPEED, 10)
        self.pub_pos = self.create_publisher(Float32, TOPIC_POS, 10)
        self.pub_status = self.create_publisher(String, TOPIC_STATUS, 10)
        self.create_subscription(Float32, TOPIC_POSE, self.on_pose, 10)
        self.create_subscription(Bool, TOPIC_ESTOP, self.on_estop, 10)
        self.cur = None
        self.estop = False

    def on_pose(self, msg):
        self.cur = msg.data

    def on_estop(self, msg):
        self.estop = bool(msg.data)

    def wait_if_estop(self):
        while rclpy.ok() and self.estop:
            self.get_logger().warn('急停已触发，等待解除…')
            rclpy.spin_once(self, timeout_sec=0.2)

    def _pub_n(self, pub, value, n=3):
        msg = pub.msg_type()            # 按发布者类型构造消息
        msg.data = value
        for _ in range(n):
            pub.publish(msg)
            rclpy.spin_once(self, timeout_sec=0.05)

    def preflight(self):
        # 等一次 current_pose，确认节点在发数据
        for _ in range(50):
            rclpy.spin_once(self, timeout_sec=0.1)
            if self.cur is not None:
                self.get_logger().info(f'升降节点在线，当前高度 {self.cur:.4f} m')
                return True
        self.get_logger().error('收不到 current_pose，请检查 Modbus 升降节点 / lift_model')
        return False

    def move_to(self, height_m, speed_mps=0.05, timeout_sec=20.0):
        assert 0.0 <= height_m <= 0.7, '目标超出 0–0.7 m，会被限幅'
        assert 0.0 < speed_mps <= 0.2, 'yako 速度上限约 0.2 m/s'

        self.wait_if_estop()
        self._pub_n(self.pub_speed, speed_mps)     # ① 先设速度
        self._pub_n(self.pub_pos, height_m)        # ② 再发位置
        self.get_logger().info(f'目标 {height_m} m，速度 {speed_mps} m/s')

        deadline = self.get_clock().now().nanoseconds + int(timeout_sec * 1e9)
        while rclpy.ok():
            rclpy.spin_once(self, timeout_sec=0.05)
            if self.estop:
                self.get_logger().error('急停触发，停止等待')
                return False
            if self.cur is not None and abs(self.cur - height_m) < TOL_M:
                self.get_logger().info(f'到位：{self.cur:.4f} m')
                return True
            if self.get_clock().now().nanoseconds > deadline:
                self.get_logger().error(f'超时未到位，当前 {self.cur:.4f} m')
                return False

    def stop(self):
        self._pub_n(self.pub_status, 'motor_stop')  # 立即停止

def main():
    rclpy.init()
    node = LiftClient()
    try:
        if node.preflight():
            node.move_to(0.10, speed_mps=0.05)      # 升到 100 mm
            node.move_to(0.00, speed_mps=0.05)      # 再降回 0
    finally:
        node.stop()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

6.7 完整示例：C++ 端到端

// lift_demo.cpp —— 升降对接端到端示例（示意代码）
#include <chrono>
#include <cmath>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;
static constexpr double TOL_M = 0.005;      // 工程建议容差，手册未给

class LiftClient : public rclcpp::Node {
 public:
  LiftClient() : Node("lift_demo") {
    pub_speed_ = create_publisher<std_msgs::msg::Float32>(
        "/fd_robot/lifting_motor/position_set_speed_cmd", 10);
    pub_pos_ = create_publisher<std_msgs::msg::Float32>(
        "/fd_robot/lifting_motor/position_cmd", 10);
    pub_status_ = create_publisher<std_msgs::msg::String>(
        "/fd_robot/lifting_motor/status", 10);

    sub_pose_ = create_subscription<std_msgs::msg::Float32>(
        "/fd_robot/lifting_motor/current_pose", 10,
        [this](std_msgs::msg::Float32::SharedPtr m) { cur_ = m->data; });
    sub_estop_ = create_subscription<std_msgs::msg::Bool>(
        "/fd_robot/emergency_signal", 10,
        [this](std_msgs::msg::Bool::SharedPtr m) { estop_ = m->data; });
  }

  bool preflight() {
    for (int i = 0; i < 50; ++i) {
      rclcpp::spin_some(this);
      if (!std::isnan(cur_)) {
        RCLCPP_INFO(get_logger(), "升降节点在线，当前高度 %.4f m", cur_);
        return true;
      }
      rclcpp::sleep_for(100ms);
    }
    RCLCPP_ERROR(get_logger(), "收不到 current_pose，请检查升降节点");
    return false;
  }

  bool move_to(double height_m, double speed_mps = 0.05,
               double timeout_sec = 20.0) {
    if (height_m < 0.0 || height_m > 0.7) {
      RCLCPP_WARN(get_logger(), "目标超出行程 0–0.7 m，会被限幅");
    }
    if (estop_) { RCLCPP_ERROR(get_logger(), "急停已触发，拒绝发送命令"); return false; }

    std_msgs::msg::Float32 s;
    s.data = static_cast<float>(speed_mps);
    for (int i = 0; i < 3; ++i) { pub_speed_->publish(s); rclcpp::sleep_for(50ms); }

    std_msgs::msg::Float32 p;
    p.data = static_cast<float>(height_m);
    for (int i = 0; i < 3; ++i) { pub_pos_->publish(p); rclcpp::sleep_for(50ms); }

    auto deadline = now() + rclcpp::Duration::from_seconds(timeout_sec);
    while (rclcpp::ok() && now() < deadline) {
      rclcpp::spin_some(this);
      if (estop_) { RCLCPP_ERROR(get_logger(), "急停触发，停止等待"); return false; }
      if (!std::isnan(cur_) && std::fabs(cur_ - height_m) < TOL_M) {
        RCLCPP_INFO(get_logger(), "到位：%.4f m", cur_);
        return true;
      }
      rclcpp::sleep_for(50ms);
    }
    RCLCPP_ERROR(get_logger(), "超时未到位，当前 %.4f m", cur_);
    return false;
  }

  void stop() {
    std_msgs::msg::String m;
    m.data = "motor_stop";
    for (int i = 0; i < 3; ++i) { pub_status_->publish(m); rclcpp::sleep_for(50ms); }
  }

 private:
  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_speed_, pub_pos_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_status_;
  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_pose_;
  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_estop_;
  double cur_{std::nan("")};
  bool estop_{false};
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<LiftClient>();
  if (node->preflight()) {
    node->move_to(0.10, 0.05);      // 升到 100 mm
    node->move_to(0.00, 0.05);      // 降回 0
  }
  node->stop();
  rclcpp::shutdown();
  return 0;
}

6.8 急停与关机场景处理

七、注意事项与已知限制

7.1 硬性约束

7.2 已知限制表

下表前 5 条来自手册附录「已知限制」与第六章正文；第 6–9 条是本文在写示例过程中发现的手册未明确项，已逐条标注。

7.3 排查速查表

7.4 通用提醒