一、概述

底盘是机器人的移动平台。它和腰部那种"一外设多套互斥驱动栈"的情况不同：手册第七章第一句就写明——slamtec、woosh、yunji 三种底盘使用相同的 ROS 话题名。

也就是说：接口不用按机型分栈，一套代码三种底盘通用。真正需要分机型的是能力，其中最关键的一条是：

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

对接底盘真正要小心的是三件事：① slamtec 不支持按点位导航，发 nav_mode=0 会立刻 failed；② nav_status 的三个标志可能短暂同时为 true，不能用"当前是否为 true"判断结果，要用 false→true 的跳变时刻；③ 三种底盘共用同名话题，所以"接口能调通"不代表"该功能这台机器支持"。

1.1 与 fd_api 底盘接口的关系

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

1.2 底盘在整机中的位置

底盘和升降一样，没有应用层服务、没有动作接口——程序直接发布话题，节点转发给底盘厂家接口。因此本章全部接口都是话题。

二、运行前提与约定

2.1 环境与节点前提

2.2 如何确认接口可用

# 1) 确认能列出底盘话题
ros2 topic list | grep -E 'chassis|slamtec'

# 期望看到（二次开发 4 条）
# /fd_robot/chassis/current_pose
# /fd_robot/chassis/goto_control
# /fd_robot/chassis/move_control
# /fd_robot/chassis/nav_status

# 2) 确认位姿反馈在发布（应约 5 Hz）
ros2 topic hz /fd_robot/chassis/current_pose

# 3) 确认消息类型与订阅者数量
ros2 topic info /fd_robot/chassis/move_control -v

# 4) 查看两个消息类型的完整定义
ros2 interface show fd_msgs/msg/ChassisMoveMsg
ros2 interface show fd_msgs/msg/ChassisGotoMsg

2.3 单位与取值约定

2.4 方向与导航模式

2.5 如何判断「到位」

手册 1.2 节接入步骤第 4 条给出标准做法：发布 goto_control 后，订阅 nav_status，根据 arrived 或 failed 判断导航结果。

但 7.3 节紧接着给了一个非常关键的细节：

所以要判断的不是「arrived 现在是 true 吗」，而是「arrived 从 false 变成 true 的那一刻」。正确与错误写法对照：

# ✗ 错误：只看当前值，三个标志同时为 true 时会误判
if nav_status.arrived:
    print('到了')

# ✓ 正确：检测 false → true 的跳变，且优先判定 failed
if not prev_failed and cur_failed:
    print('导航失败')
elif not prev_arrived and cur_arrived:
    print('已到达')

2.6 安全前提与已知交互

三、接口总览

3.1 二次开发接口（4 条）

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

3.2 系统内部接口（4 条）

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

3.3 关于服务与动作

四、话题接口详解

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

示例

① 命令行（ros2 CLI）

# 前进 1 秒，线速度 0.2 m/s
ros2 topic pub -1 /fd_robot/chassis/move_control fd_msgs/msg/ChassisMoveMsg \
  "{direction: 0, duration: 1000, linear_speed: 0.2, angular_speed: 0.0}"

# 原地右转 0.5 秒（direction=2 是右转、3 是左转）
ros2 topic pub -1 /fd_robot/chassis/move_control fd_msgs/msg/ChassisMoveMsg \
  "{direction: 2, duration: 500, linear_speed: 0.0, angular_speed: 0.5}"

# 速度填 0，使用节点默认值（约 0.2 m/s）
ros2 topic pub -1 /fd_robot/chassis/move_control fd_msgs/msg/ChassisMoveMsg \
  "{direction: 0, duration: 1000, linear_speed: 0.0, angular_speed: 0.0}"

# 查看消息定义
ros2 interface show fd_msgs/msg/ChassisMoveMsg

② Python（rclpy）

# 示意代码：定时速度运动，并在 duration 内等待、结束后读位姿
import time
import rclpy
from rclpy.node import Node
from fd_msgs.msg import ChassisMoveMsg
from geometry_msgs.msg import Pose

DIR_FORWARD, DIR_BACKWARD, DIR_RIGHT, DIR_LEFT = 0, 1, 2, 3

class ChassisMove(Node):
    def __init__(self):
        super().__init__('chassis_move')
        self.pub = self.create_publisher(
            ChassisMoveMsg, '/fd_robot/chassis/move_control', 10)
        self.sub = self.create_subscription(
            Pose, '/fd_robot/chassis/current_pose', self.on_pose, 10)
        self.pose = None

    def on_pose(self, msg):
        self.pose = msg

    def move(self, direction, duration_ms, linear_speed=0.0, angular_speed=0.0):
        msg = ChassisMoveMsg()
        msg.direction = int(direction)
        msg.duration = int(duration_ms)          # 毫秒
        msg.linear_speed = float(linear_speed)   # ≤0 用节点默认值
        msg.angular_speed = float(angular_speed)
        for _ in range(3):
            self.pub.publish(msg)
            rclpy.spin_once(self, timeout_sec=0.05)
        self.get_logger().info(
            f'已下发 direction={direction} duration={duration_ms} ms，到时自动停止')

        # 手册：到时自动停止。等待期内保持 spin，便于接收位姿
        end = time.time() + duration_ms / 1000.0
        while time.time() < end and rclpy.ok():
            rclpy.spin_once(self, timeout_sec=0.05)

        if self.pose is not None:
            p = self.pose.position
            self.get_logger().info(f'运动结束，位姿 x={p.x:.3f} y={p.y:.3f}')

def main():
    rclpy.init()
    node = ChassisMove()
    node.move(DIR_FORWARD, 1000, linear_speed=0.2)   # 前进 1 秒
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include "fd_msgs/msg/chassis_move_msg.hpp"
#include "geometry_msgs/msg/pose.hpp"

using namespace std::chrono_literals;

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

  auto pub = node->create_publisher<fd_msgs::msg::ChassisMoveMsg>(
      "/fd_robot/chassis/move_control", 10);

  auto sub = node->create_subscription<geometry_msgs::msg::Pose>(
      "/fd_robot/chassis/current_pose", 10,
      [](geometry_msgs::msg::Pose::SharedPtr m) {
        RCLCPP_INFO_THROTTLE(rclcpp::get_logger("chassis_move"),
                             *rclcpp::Clock::make_shared(), 1000,
                             "pose x=%.3f y=%.3f", m->position.x, m->position.y);
      });

  fd_msgs::msg::ChassisMoveMsg cmd;
  cmd.direction = 0;            // 0 前进 / 1 后退 / 2 右转 / 3 左转
  cmd.duration = 1000;          // 毫秒，到时自动停止
  cmd.linear_speed = 0.2f;      // m/s，≤0 用节点默认值
  cmd.angular_speed = 0.0f;     // rad/s

  for (int i = 0; i < 3; ++i) { pub->publish(cmd); rclcpp::sleep_for(50ms); }
  RCLCPP_INFO(node->get_logger(), "已下发前进 1000 ms");

  // duration 内保持 spin，便于接收位姿反馈
  auto end = node->now() + 1s;
  while (rclcpp::ok() && node->now() < end) {
    rclcpp::spin_some(node);
    rclcpp::sleep_for(50ms);
  }

  rclcpp::shutdown();
  return 0;
}

示例

① 命令行（ros2 CLI）

# ① 按位姿导航（三种底盘都支持）
ros2 topic pub -1 /fd_robot/chassis/goto_control fd_msgs/msg/ChassisGotoMsg \
  "{nav_mode: 1, request_type: 0, x: 1.0, y: 0.0, yaw: 0.0}"

# ② 按点位导航（仅 woosh / yunji；slamtec 会立即 failed）
ros2 topic pub -1 /fd_robot/chassis/goto_control fd_msgs/msg/ChassisGotoMsg \
  "{nav_mode: 0, request_type: 0, mark_no: '1'}"

# ③ 取消导航
ros2 topic pub -1 /fd_robot/chassis/goto_control fd_msgs/msg/ChassisGotoMsg \
  "{nav_mode: 1, request_type: 1}"

# 查看消息定义
ros2 interface show fd_msgs/msg/ChassisGotoMsg

② Python（rclpy）

# 示意代码：三种操作各封装一个方法；同时订阅 nav_status 打印状态跳变
import rclpy
from rclpy.node import Node
from fd_msgs.msg import ChassisGotoMsg, ChassisNavStatusMsg

class ChassisGoto(Node):
    def __init__(self):
        super().__init__('chassis_goto')
        self.pub = self.create_publisher(
            ChassisGotoMsg, '/fd_robot/chassis/goto_control', 10)
        self.sub = self.create_subscription(
            ChassisNavStatusMsg, '/fd_robot/chassis/nav_status',
            self.on_status, 10)
        self.prev = (False, False, False)

    def on_status(self, msg):
        cur = (bool(msg.active), bool(msg.arrived), bool(msg.failed))
        # 手册：三个标志可能短暂同时为 true，以 false→true 的跳变时刻为准
        if cur[2] and not self.prev[2]:
            self.get_logger().warn('nav_status: failed 触发')
        elif cur[1] and not self.prev[1]:
            self.get_logger().info('nav_status: arrived 触发')
        self.prev = cur

    def _send(self, msg):
        for _ in range(3):
            self.pub.publish(msg)
            rclpy.spin_once(self, timeout_sec=0.05)

    def goto_pose(self, x, y, yaw, cancel=False):
        msg = ChassisGotoMsg()
        msg.nav_mode = 1                      # POSE，三种底盘都支持
        msg.request_type = 1 if cancel else 0
        msg.x = float(x); msg.y = float(y); msg.yaw = float(yaw)
        self._send(msg)
        self.get_logger().info(
            f"{'取消' if cancel else '开始'}位姿导航 x={x} y={y} yaw={yaw}")

    def goto_mark(self, mark_no):
        msg = ChassisGotoMsg()
        msg.nav_mode = 0                      # MARK，仅 woosh / yunji
        msg.request_type = 0
        msg.mark_no = str(mark_no)
        self._send(msg)
        self.get_logger().info(f'开始点位导航 mark_no={mark_no}')

def main():
    rclpy.init()
    node = ChassisGoto()
    node.goto_pose(1.0, 0.0, 0.0)             # 去 (1.0, 0.0) 朝向 0
    # 需要取消时：node.goto_pose(0, 0, 0, cancel=True)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include "fd_msgs/msg/chassis_goto_msg.hpp"

using namespace std::chrono_literals;

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("chassis_goto");
  auto pub = node->create_publisher<fd_msgs::msg::ChassisGotoMsg>(
      "/fd_robot/chassis/goto_control", 10);

  // 按位姿导航（nav_mode=1，三种底盘均支持）
  fd_msgs::msg::ChassisGotoMsg goal;
  goal.nav_mode = 1;
  goal.request_type = 0;        // 0 开始 / 1 取消
  goal.x = 1.0;                 // m，地图坐标系
  goal.y = 0.0;
  goal.yaw = 0.0;               // rad
  // task_type / task_id / direction / task_type_no 二次开发保持 0 即可

  for (int i = 0; i < 3; ++i) { pub->publish(goal); rclcpp::sleep_for(50ms); }
  RCLCPP_INFO(node->get_logger(), "已下发位姿导航 (1.0, 0.0, 0.0)");

  // 需要取消时：
  // fd_msgs::msg::ChassisGotoMsg cancel;
  // cancel.nav_mode = 1;
  // cancel.request_type = 1;
  // pub->publish(cancel);

  rclcpp::shutdown();
  return 0;
}

示例

① 命令行（ros2 CLI）

# 持续观察导航状态（发导航命令后才有数据）
ros2 topic echo /fd_robot/chassis/nav_status

# 只取一次
ros2 topic echo --once /fd_robot/chassis/nav_status

# 查看消息定义（确认是否有手册未列出的字段）
ros2 interface show fd_msgs/msg/ChassisNavStatusMsg

# 典型联合调试：先发导航，再看状态
ros2 topic pub -1 /fd_robot/chassis/goto_control fd_msgs/msg/ChassisGotoMsg \
  "{nav_mode: 1, request_type: 0, x: 1.0, y: 0.0, yaw: 0.0}"
ros2 topic echo /fd_robot/chassis/nav_status

② Python（rclpy）

# 示意代码：状态机 + 跳变判定 + 超时保护
import rclpy
from rclpy.node import Node
from fd_msgs.msg import ChassisNavStatusMsg

class NavWatcher(Node):
    def __init__(self):
        super().__init__('nav_watcher')
        self.sub = self.create_subscription(
            ChassisNavStatusMsg, '/fd_robot/chassis/nav_status',
            self.on_status, 10)
        self.prev = (False, False, False)
        self.result = None          # 'arrived' / 'failed' / None

    def on_status(self, msg):
        active, arrived, failed = bool(msg.active), bool(msg.arrived), bool(msg.failed)
        # 手册 7.3：三个标志可能短暂同时为 true，
        # 以 failed 或 arrived 由 false 变为 true 的时刻作为判定依据
        if failed and not self.prev[2]:
            self.get_logger().warn('判定：导航失败 (failed 由 false→true)')
            self.result = 'failed'
        elif arrived and not self.prev[1]:
            self.get_logger().info('判定：已到达 (arrived 由 false→true)')
            self.result = 'arrived'
        elif active:
            self.get_logger().debug('导航进行中')
        self.prev = (active, arrived, failed)

    def wait(self, timeout_sec=120.0):
        deadline = self.get_clock().now().nanoseconds + int(timeout_sec * 1e9)
        while rclpy.ok() and self.result is None:
            rclpy.spin_once(self, timeout_sec=0.1)
            if self.get_clock().now().nanoseconds > deadline:
                self.get_logger().error('等待导航结果超时')
                return None
        return self.result

def main():
    rclpy.init()
    node = NavWatcher()
    result = node.wait(120.0)
    node.get_logger().info(f'导航结果：{result}')
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

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

using namespace std::chrono_literals;

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

  bool prev_arrived = false, prev_failed = false;
  std::string result = "none";

  auto sub = node->create_subscription<fd_msgs::msg::ChassisNavStatusMsg>(
      "/fd_robot/chassis/nav_status", 10,
      [&](fd_msgs::msg::ChassisNavStatusMsg::SharedPtr m) {
        // 手册 7.3：三个标志可能短暂同时为 true，
        // 以 failed 或 arrived 由 false 变为 true 的时刻作为判定依据
        if (m->failed && !prev_failed) {
          RCLCPP_WARN(rclcpp::get_logger("nav_watcher"),
                      "判定：导航失败 (failed false->true)");
          result = "failed";
        } else if (m->arrived && !prev_arrived) {
          RCLCPP_INFO(rclcpp::get_logger("nav_watcher"),
                      "判定：已到达 (arrived false->true)");
          result = "arrived";
        }
        prev_arrived = m->arrived;
        prev_failed = m->failed;
      });

  auto deadline = node->now() + 120s;      // 自行设置超时，手册未给导航超时约定
  while (rclcpp::ok() && node->now() < deadline && result == "none") {
    rclcpp::spin_some(node);
    rclcpp::sleep_for(100ms);
  }
  RCLCPP_INFO(node->get_logger(), "导航结果：%s", result.c_str());

  rclcpp::shutdown();
  return 0;
}

示例

① 命令行（ros2 CLI）

# 持续观察位姿（应约 5 Hz）
ros2 topic echo /fd_robot/chassis/current_pose

# 只取一次当前位置
ros2 topic echo --once /fd_robot/chassis/current_pose

# 确认发布频率
ros2 topic hz /fd_robot/chassis/current_pose

② Python（rclpy）

# 示意代码：订阅位姿并换算 yaw（弧度）
import math
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose

class PoseMonitor(Node):
    def __init__(self):
        super().__init__('pose_monitor')
        self.sub = self.create_subscription(
            Pose, '/fd_robot/chassis/current_pose', self.on_pose, 10)
        self.pose = None

    @staticmethod
    def quat_to_yaw(q):
        # 标准四元数 → yaw 换算（通用数学关系，手册未给出）
        return math.atan2(2.0 * (q.w * q.z + q.x * q.y),
                          1.0 - 2.0 * (q.y * q.y + q.z * q.z))

    def on_pose(self, msg):
        self.pose = msg
        yaw = self.quat_to_yaw(msg.orientation)
        self.get_logger().info(
            f'x={msg.position.x:.3f} y={msg.position.y:.3f} '
            f'yaw={yaw:.3f} rad ({math.degrees(yaw):.1f}°)')

def main():
    rclpy.init()
    node = PoseMonitor()
    rclpy.spin(node)          # 持续接收
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

③ C++（rclcpp）

#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include "geometry_msgs/msg/pose.hpp"

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

  auto sub = node->create_subscription<geometry_msgs::msg::Pose>(
      "/fd_robot/chassis/current_pose", 10,
      [](geometry_msgs::msg::Pose::SharedPtr m) {
        // 标准四元数 → yaw（通用数学关系，手册未给出）
        const auto& q = m->orientation;
        double yaw = std::atan2(2.0 * (q.w * q.z + q.x * q.y),
                                1.0 - 2.0 * (q.y * q.y + q.z * q.z));
        RCLCPP_INFO(rclcpp::get_logger("pose_monitor"),
                    "x=%.3f y=%.3f yaw=%.3f rad (%.1f deg)",
                    m->position.x, m->position.y, yaw, yaw * 180.0 / M_PI);
      });

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

五、系统内部接口

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

ros2 topic echo /fd_robot/chassis_status
ros2 interface show fd_msgs/msg/ChassisStatusMsg

ros2 topic echo /fd_robot/slamtec/battery_info
ros2 interface show fd_msgs/msg/SlamTecBatteryMsg

ros2 topic echo /fd_robot/slamtec/run_status

ros2 topic info /fd_robot/shutdown_signal -v   # 看清发布者是谁
ros2 topic echo /fd_robot/shutdown_signal

六、典型流程与完整示例

6.1 首次对接步骤

6.2 两种运动方式怎么选

6.3 定时速度运动的标准姿势

6.4 导航完整流程

6.5 取消导航

# 取消导航：同一个 goto_control 话题，request_type=1
ros2 topic pub -1 /fd_robot/chassis/goto_control fd_msgs/msg/ChassisGotoMsg \
  "{nav_mode: 1, request_type: 1}"

# 取消后观察状态，确认已停止
ros2 topic echo /fd_robot/chassis/nav_status
ros2 topic echo /fd_robot/chassis/current_pose

6.6 三种底盘支持矩阵与降级策略

6.7 完整示例：Python 端到端

下面这份脚本把「前置检查 → 先订阅 → 导航 → 跳变判定 → 超时取消」串成了完整链路（示意代码，请按实际机型调整超时与目标点）。

# chassis_nav_demo.py —— 底盘导航端到端示例（示意代码）
import rclpy
from rclpy.node import Node
from fd_msgs.msg import ChassisGotoMsg, ChassisNavStatusMsg
from geometry_msgs.msg import Pose

TOPIC_GOTO = '/fd_robot/chassis/goto_control'
TOPIC_NAV = '/fd_robot/chassis/nav_status'
TOPIC_POSE = '/fd_robot/chassis/current_pose'

class ChassisNav(Node):
    def __init__(self):
        super().__init__('chassis_nav_demo')
        self.pub = self.create_publisher(ChassisGotoMsg, TOPIC_GOTO, 10)
        self.sub_nav = self.create_subscription(
            ChassisNavStatusMsg, TOPIC_NAV, self.on_nav, 10)
        self.sub_pose = self.create_subscription(
            Pose, TOPIC_POSE, self.on_pose, 10)
        self.prev = (False, False, False)
        self.result = None
        self.pose = None

    def on_pose(self, msg):
        self.pose = msg

    def on_nav(self, msg):
        active, arrived, failed = bool(msg.active), bool(msg.arrived), bool(msg.failed)
        # 手册 7.3：三个标志可能短暂同时为 true，
        # 以 failed 或 arrived 由 false 变为 true 的时刻作为判定依据
        if failed and not self.prev[2]:
            self.get_logger().warn('导航失败（failed false→true）')
            self.result = 'failed'
        elif arrived and not self.prev[1]:
            self.get_logger().info('已到达（arrived false→true）')
            self.result = 'arrived'
        self.prev = (active, arrived, failed)

    def preflight(self):
        for _ in range(50):
            rclpy.spin_once(self, timeout_sec=0.1)
            if self.pose is not None:
                self.get_logger().info(
                    f'底盘在线，起点 x={self.pose.position.x:.3f} '
                    f'y={self.pose.position.y:.3f}')
                return True
        self.get_logger().error('收不到 current_pose，请检查底盘驱动是否启动')
        return False

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

    def goto_pose(self, x, y, yaw, timeout_sec=120.0):
        self.result = None
        msg = ChassisGotoMsg()
        msg.nav_mode = 1            # POSE：三种底盘均支持
        msg.request_type = 0        # 0 开始
        msg.x, msg.y, msg.yaw = float(x), float(y), float(yaw)
        self._send(msg)
        self.get_logger().info(f'开始导航到 x={x} y={y} yaw={yaw}')

        deadline = self.get_clock().now().nanoseconds + int(timeout_sec * 1e9)
        while rclpy.ok() and self.result is None:
            rclpy.spin_once(self, timeout_sec=0.1)
            if self.get_clock().now().nanoseconds > deadline:
                self.get_logger().error('导航超时，取消导航')
                self.cancel_nav()
                return 'timeout'
        return self.result

    def cancel_nav(self):
        msg = ChassisGotoMsg()
        msg.nav_mode = 1
        msg.request_type = 1        # 1 取消
        self._send(msg)
        self.get_logger().warn('已下发取消导航')

def main():
    rclpy.init()
    node = ChassisNav()
    try:
        if node.preflight():
            result = node.goto_pose(1.0, 0.0, 0.0, timeout_sec=120.0)
            node.get_logger().info(f'导航结果：{result}')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

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

// chassis_nav_demo.cpp —— 底盘导航端到端示例（示意代码）
#include <chrono>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include "fd_msgs/msg/chassis_goto_msg.hpp"
#include "fd_msgs/msg/chassis_nav_status_msg.hpp"
#include "geometry_msgs/msg/pose.hpp"

using namespace std::chrono_literals;

class ChassisNav : public rclcpp::Node {
 public:
  ChassisNav() : Node("chassis_nav_demo") {
    pub_ = create_publisher<fd_msgs::msg::ChassisGotoMsg>(
        "/fd_robot/chassis/goto_control", 10);

    sub_nav_ = create_subscription<fd_msgs::msg::ChassisNavStatusMsg>(
        "/fd_robot/chassis/nav_status", 10,
        [this](fd_msgs::msg::ChassisNavStatusMsg::SharedPtr m) {
          // 手册 7.3：三个标志可能短暂同时为 true，以 false→true 的跳变时刻为准
          if (m->failed && !prev_failed_) {
            RCLCPP_WARN(get_logger(), "导航失败（failed false->true）");
            result_ = "failed";
          } else if (m->arrived && !prev_arrived_) {
            RCLCPP_INFO(get_logger(), "已到达（arrived false->true）");
            result_ = "arrived";
          }
          prev_arrived_ = m->arrived;
          prev_failed_ = m->failed;
        });

    sub_pose_ = create_subscription<geometry_msgs::msg::Pose>(
        "/fd_robot/chassis/current_pose", 10,
        [this](geometry_msgs::msg::Pose::SharedPtr m) {
          has_pose_ = true;
          RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000,
                               "pose x=%.3f y=%.3f",
                               m->position.x, m->position.y);
        });
  }

  bool preflight() {
    for (int i = 0; i < 50 && rclcpp::ok(); ++i) {
      rclcpp::spin_some(this);
      if (has_pose_) { RCLCPP_INFO(get_logger(), "底盘在线"); return true; }
      rclcpp::sleep_for(100ms);
    }
    RCLCPP_ERROR(get_logger(), "收不到 current_pose，请检查底盘驱动是否启动");
    return false;
  }

  std::string goto_pose(double x, double y, double yaw, double timeout_sec) {
    result_ = "none";
    fd_msgs::msg::ChassisGotoMsg goal;
    goal.nav_mode = 1;          // POSE：三种底盘均支持
    goal.request_type = 0;      // 0 开始
    goal.x = x; goal.y = y; goal.yaw = yaw;
    for (int i = 0; i < 3; ++i) { pub_->publish(goal); rclcpp::sleep_for(50ms); }

    auto deadline = now() + rclcpp::Duration::from_seconds(timeout_sec);
    while (rclcpp::ok() && now() < deadline && result_ == "none") {
      rclcpp::spin_some(this);
      rclcpp::sleep_for(100ms);
    }
    if (result_ == "none") { cancel_nav(); return "timeout"; }
    return result_;
  }

  void cancel_nav() {
    fd_msgs::msg::ChassisGotoMsg cancel;
    cancel.nav_mode = 1;
    cancel.request_type = 1;    // 1 取消
    for (int i = 0; i < 3; ++i) { pub_->publish(cancel); rclcpp::sleep_for(50ms); }
    RCLCPP_WARN(get_logger(), "已下发取消导航");
  }

 private:
  rclcpp::Publisher<fd_msgs::msg::ChassisGotoMsg>::SharedPtr pub_;
  rclcpp::Subscription<fd_msgs::msg::ChassisNavStatusMsg>::SharedPtr sub_nav_;
  rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr sub_pose_;
  bool prev_arrived_{false}, prev_failed_{false}, has_pose_{false};
  std::string result_{"none"};
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ChassisNav>();
  if (node->preflight()) {
    std::string r = node->goto_pose(1.0, 0.0, 0.0, 120.0);
    RCLCPP_INFO(node->get_logger(), "导航结果：%s", r.c_str());
  }
  rclcpp::shutdown();
  return 0;
}

6.9 与整机状态 / 安全的联动

手册附录给了一条容易被忽略的限制：/fd_robot/robot_idle_status 可能没有把底盘运动算进去。也就是说，「整机空闲」为 true 时，底盘可能正在移动。

# 示意代码：判断"真的都停了"，不要只信 robot_idle_status
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool, String

class IdleChecker(Node):
    def __init__(self):
        super().__init__('idle_checker')
        self.create_subscription(String, '/fd_robot/robot_idle_status',
                                 self.on_idle, 10)
        self.create_subscription(Bool, '/fd_robot/slamtec/run_status',
                                 self.on_run, 10)
        self.idle_json = None
        self.chassis_moving = None

    def on_idle(self, msg):
        self.idle_json = msg.data

    def on_run(self, msg):
        self.chassis_moving = bool(msg.data)

    def is_really_idle(self):
        # 附录：底盘运动可能未计入空闲判断，需结合 run_status / nav_status
        if self.chassis_moving:
            return False
        if self.idle_json is None:
            return False
        return '"idle"' in self.idle_json and 'true' in self.idle_json

def main():
    rclpy.init()
    node = IdleChecker()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

七、注意事项与已知限制

7.1 硬性约束

7.2 已知限制表

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

7.3 排查速查表

7.4 通用提醒