纳博特科技开放平台

中文

English
한국어

中文

English
한국어

Appearance

3. 伺服状态检测 ​

本期将介绍如何连接控制器，查询机器人当前的伺服状态，并根据状态码输出对应的检测结果。

在执行上电、运动或其他控制操作之前，通常需要先确认伺服当前所处的状态。程序通过 get_servo_state 获取状态码，再根据状态码判断机器人是未上电、已经就绪、正在运行，还是处于报警状态。

伺服状态码的含义如下：

  • 状态 0：伺服未上电
  • 状态 1：伺服就绪
  • 状态 2：伺服报警
  • 状态 3：伺服运行中

⚠️ 使用注意

本 Demo 只查询和显示伺服状态，不会修改伺服状态，也不会向机器人发送运动指令。运行前仍需确认：

  • 连接配置： 控制器 IP 地址和 SDK 服务端口与实际配置一致
  • 查询结果： 只有 get_servo_state 返回 SUCCESS 时，才能使用查询得到的状态值
  • 报警处理： 状态为 2 时应停止后续控制操作，并检查控制器报警信息
  • 未知状态： SDK 返回未定义状态码时，应按异常处理，不能继续下发控制命令
  • 现场安全： 即使本 Demo 不控制机器人，也应确保急停和安全回路处于正常状态

伺服状态只能反映当前查询时刻的结果。实际项目在执行关键控制操作前，应根据业务需要重新查询状态，避免使用已经过期的状态值。

1、配置控制器连接和查询参数 ​

首先配置控制器 IP 地址、SDK 服务端口、查询失败后的重试间隔，以及允许的最大连续查询失败次数。

cpp
/**
 * @name 用户必须根据运行环境修改
 * 以下配置决定 SDK 能否连接到实际控制器，运行前必须核对。
 * @{
 */
const std::string robot_ip = "192.168.3.243";   ///< 用户实际控制器的 IP 地址。
const std::string robot_port = "6001";          ///< 用户实际控制器的 SDK 服务端口，必须与控制器配置一致。
/** @} */

/**
 * @name 建议客户修改
 * 以下配置应结合控制器通信质量和允许的查询响应时间进行调整。
 * @{
 */
constexpr int query_retry_interval_ms = 500;     ///< 查询失败后的重试间隔，单位为毫秒。
/** @} */

/**
 * @name 可修改也可保留默认值
 * 默认值与本 Demo 的失败提示一致，一般情况下可直接保留。
 * @{
 */
constexpr int max_query_failures = 3;            ///< 单次状态查询允许的最大连续失败次数。
/** @} */

query_retry_interval_ms 用于控制两次查询之间的等待时间。如果间隔过短，可能增加控制器的通信负载；如果间隔过长，则会延长状态检测的总响应时间。

max_query_failures 用于限制单次检测允许尝试的最大次数，避免控制器持续无响应时程序无限重试。

2、封装带重试的伺服状态查询函数 ​

网络短暂波动或控制器临时繁忙时，单次调用 get_servo_state 可能失败。为了避免一次查询失败就直接结束检测，程序封装了 get_servo_state_with_retry 函数，在配置的次数范围内重复查询。

cpp
/**
 * @brief 查询当前伺服状态，并在查询失败时按配置进行重试。
 * @param[in] fd 控制器连接句柄。
 * @param[out] state 查询成功时保存当前伺服状态码。
 * @retval true 在允许的重试次数内成功获取伺服状态。
 * @retval false 所有查询均失败，此时调用方不应使用 @p state。
 */
bool get_servo_state_with_retry(SOCKETFD fd, int& state)
{
    // 按固定次数处理临时网络波动，避免单次查询失败就结束检测。
    for (int attempt = 1; attempt <= max_query_failures; ++attempt)
    {
        // SDK 将当前伺服状态写入 state，并返回本次调用结果。
        const Result result = get_servo_state(fd, state);
        if (result == SUCCESS)
        {
            return true;  // 查询成功，此时 state 已保存当前状态值。
        }

        std::cerr << "获取伺服状态失败，第 "
                  << attempt << "/" << max_query_failures
                  << " 次，错误码：" << static_cast<int>(result) << std::endl;

        if (attempt < max_query_failures)
        {
            std::this_thread::sleep_for(
                std::chrono::milliseconds(query_retry_interval_ms));
        }
    }

    // 所有查询均失败，调用方不应再依据 state 执行后续判断。
    std::cerr << "连续3次获取伺服状态失败" << std::endl;
    return false;
}

函数的执行过程如下：

  1. 从第 1 次查询开始调用 get_servo_state。
  2. 如果接口返回 SUCCESS，立即返回 true，此时 state 保存有效状态码。
  3. 如果查询失败，输出当前失败次数和 SDK 错误码。
  4. 未达到最大次数时，等待 query_retry_interval_ms 毫秒后再次查询。
  5. 所有查询均失败时返回 false，调用方不能继续解释 state。

3、封装伺服状态展示与判断函数 ​

成功取得状态码后，还需要将数值状态转换为便于理解的文字，并判断该状态是否允许程序继续执行后续控制操作。

单独封装 show_servo_state，是为了将“从控制器读取状态”和“解释状态含义”分离。查询函数只负责保证数据有效，展示函数统一维护状态码与文字说明的对应关系，并通过布尔返回值告知主程序当前状态是否正常。以后增加新的状态码或修改异常处理方式时，只需要调整该函数。

cpp
/**
 * @brief 输出 SDK 状态码对应的伺服状态，并判断是否允许继续后续控制操作。
 * @param[in] state SDK 返回的伺服状态码。
 * @retval true 伺服处于未上电、就绪或运行中状态。
 * @retval false 伺服处于报警状态，或 SDK 返回未知状态码。
 */
bool show_servo_state(int state)
{
    switch (state)
    {
    case 0:
        std::cout << "伺服状态：未上电" << std::endl;
        return true;

    case 1:
        std::cout << "伺服状态：就绪" << std::endl;
        return true;

    case 2:
        std::cerr << "伺服状态：报警" << std::endl;
        std::cerr << "处理：停止后续控制操作，请检查控制器报警信息"
                  << std::endl;
        return false;  // 报警未排除时停止后续控制，避免继续下发命令。

    case 3:
        std::cout << "伺服状态：运行中" << std::endl;
        return true;

    default:
        // 未定义的状态码同样按异常处理，避免根据未知状态作出错误判断。
        std::cerr << "伺服返回未知状态：" << state << std::endl;
        std::cerr << "处理：停止后续控制操作" << std::endl;
        return false;
    }
}

各状态的处理逻辑如下：

  1. 状态 0：输出“未上电”，状态值有效，函数返回 true。
  2. 状态 1：输出“就绪”，状态值有效，函数返回 true。
  3. 状态 2：输出“报警”和处理提示，函数返回 false，阻止后续控制操作。
  4. 状态 3：输出“运行中”，状态值有效，函数返回 true。
  5. 未知状态：无法确认机器人状态，按异常处理并返回 false。

这里返回 true 表示状态码已被识别且不属于报警或未知状态，并不表示伺服一定已经上电。例如状态 0 虽然返回 true，但它仍表示伺服当前处于未上电状态。

4、主程序连接控制器并检测伺服状态 ​

主函数负责启用 UTF-8 控制台输出、连接控制器、获取伺服状态，并根据检测结果设置程序退出码。

只有 get_servo_state_with_retry 返回成功时，主函数才会调用 show_servo_state。这样可以避免查询失败后继续使用无效或过期的状态值。

cpp
#include <iostream>
#include <thread>
#include <chrono>
#include <cpp_interface/nrc_interface.h>
#include "../demo_utils.h"

/**
 * @brief Demo 程序入口：连接控制器、查询伺服状态并输出检测结果。
 * @return 状态正常时返回 0；查询失败、报警或未知状态时返回 1。
 */
int main()
{
    demo::enable_console_utf8();  // 启用 UTF-8 控制台输出。

    SOCKETFD fd = connect_robot(robot_ip, robot_port);
    if (fd <= 0)
    {
        std::cout << "控制器连接失败" << std::endl;
        return 0;
    }
    std::cout << "控制器连接成功" << std::endl;

    int state = 0;
    bool statusNormal = false;  // 记录状态是否正常，用于确定程序最终退出码。

    if (get_servo_state_with_retry(fd, state))
    {
        // 仅在查询成功时解释状态，避免使用无效或过期的状态值。
        statusNormal = show_servo_state(state);
    }

    if (statusNormal)  // 正常状态返回 0；查询失败、报警或未知状态返回非 0。
        return 0;
    else
        return 1;
}