跳到主要内容

GR-Power

1. 概述​

GR-Power 是机器人电源状态的 DDS 发布模块。运行时应用 fourierpower 将电源 / 电池状态发布为 DDS topic，供上层模块订阅。

元素数值
运行时应用名fourierpower
Domain ID123
主要 DDS topichal_battery_monitor_status、hal_bms_error_code
运行环境

fourierpower 在机器人上以系统用户运行、开机自启。二开订阅脚本以普通用户运行即可，无需 root/sudo。与 root 服务同机通信必须在创建任何 DDS participant 前设置：

Bash
export FASTDDS_BUILTIN_TRANSPORTS=UDPv4

否则 Fast DDS 可能选择同机共享内存传输，出现端点已匹配但收不到数据（或间歇性收不到数据）的情况。下文 Python 示例已内置该设置。

2. DDS 接口​

2.1 发布 Topics​

Topic消息类型说明
hal_battery_monitor_statusfourier_msgs/msg/BmsStatusMsg主电池与副电池状态列表。
hal_bms_error_codefourier_msgs/msg/ErrorCodes电源 / 电池故障码列表。

发布节奏：hal_battery_monitor_status 约 5 Hz 持续发布；hal_bms_error_code 仅在存在故障码时发布（健康机器人上订阅不到回调属正常现象），出现故障时约 1 Hz 持续更新。

订阅端使用默认 QoS（Best Effort + Volatile）即可；不要使用 Reliable QoS——发布端为 Best Effort，Reliable 订阅无法匹配，将收不到任何数据。

2.2 订阅 Topics​

该模块暂无订阅入口

3. 消息定义​

3.1 BmsStatusMsg​

Idl
struct BmsStatus {
    double voltage;
    double current;
    double temperature;
    uint8  level;
    uint8  level_cap;
    double mos_temperature;
    boolean failure;
    boolean battery_comm;
    boolean output_mos_open;
    boolean input_mos_open;
};

struct BmsStatusMsg {
    std_msgs::msg::Header header;
    sequence<BmsStatus> bms_state_list;
};

bms_state_list[0] 表示主电池，bms_state_list[1] 表示副电池。

GR-W 字段说明

GR-W 为双电池，bms_state_list 长度为 2。当前电池源只填充 level、voltage、current、battery_comm 四个字段（battery_comm 恒为 true）；temperature、failure、mos_temperature 及 MOS 状态字段保持默认值，不要将它们用作 GR-W 的电池健康判据。

3.2 ErrorCodes​

Idl
struct ErrorCode {
    uint32 high32;
    uint32 low32;
};

struct ErrorCodes {
    std_msgs::msg::Header header;
    uint8 source; // 4 = power / battery
    sequence<ErrorCode> error_codes;
};

GR-Power 对 high32 的编码方式：

Text
level     = high32 & 0xF
code      = (high32 >> 12) & 0xFFFF
type_flag = (high32 >> 28) & 0xF
type_flag含义
0BMS1 电池故障
1BMS2 电池故障
警告

type_flag 还存在上表未列出的取值（如 2），其含义以电源模块实际固件为准；上表仅列出已定义的取值。

4. 基础示例​

4.1 Python - 订阅电池状态​

Python
import os
import signal
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")  # 机器人上运行必须设置

from fourierdds_py import DDSInterface, SubscriberQosProfile
import fourier_msgs.msg.BmsState as BmsState

dds = DDSInterface(domain_id=123)


def seq_items(seq):
    size = seq.size() if hasattr(seq, "size") else len(seq)
    return [seq[i] for i in range(size)]


def on_bms(msg):
    states = seq_items(msg.bms_state_list())
    if not states:
        print("[bms] empty state list")
        return

    for index, state in enumerate(states):
        name = "main" if index == 0 else f"sub{index}"
        print(
            f"[bms:{name}] "
            f"level={state.level()}% "
            f"voltage={state.voltage():.3f}V "
            f"current={state.current():.3f}A "
            f"temperature={state.temperature():.1f}C "
            f"failure={state.failure()} "
            f"comm={state.battery_comm()}"
        )


running = {"value": True}


def _stop(signum, frame):
    running["value"] = False
    print(f"\nsignal {signum} received, stopping ...", flush=True)


# fastdds 的原生信号处理会吞掉 Ctrl+C，必须显式覆盖才能正常退出
signal.signal(signal.SIGINT, _stop)
signal.signal(signal.SIGTERM, _stop)

dds.create_subscription(
    BmsState.BmsStatusMsgPubSubType,
    "hal_battery_monitor_status",
    on_bms,
    qos_profile=SubscriberQosProfile.default(),
)

try:
    while running["value"]:
        time.sleep(0.1)
finally:
    dds.close()   # 释放 DDS 资源；不调用可能在退出阶段报错

运行示例（Ctrl+C 停止）：

Bash
python3 watch_grpower_status.py

订阅脚本以普通用户运行即可。如果确需以 root 运行，保留 FASTDDS_BUILTIN_TRANSPORTS=UDPv4 环境变量：

Bash
sudo FASTDDS_BUILTIN_TRANSPORTS=UDPv4 python3 -u watch_grpower_status.py

4.2 Python - 订阅故障码​

Python
import os
import signal
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")  # 机器人上运行必须设置

from fourierdds_py import DDSInterface, SubscriberQosProfile
import fourier_msgs.msg.ErrorCodes as ErrorCodes

dds = DDSInterface(domain_id=123)


def seq_items(seq):
    size = seq.size() if hasattr(seq, "size") else len(seq)
    return [seq[i] for i in range(size)]


def decode_high32(high32):
    return {
        "level": high32 & 0xF,
        "code": (high32 >> 12) & 0xFFFF,
        "type_flag": (high32 >> 28) & 0xF,
    }


def on_error(msg):
    print(f"[power_error] source={msg.source()} count={len(seq_items(msg.error_codes()))}")
    for index, item in enumerate(seq_items(msg.error_codes())):
        decoded = decode_high32(item.high32())
        print(
            f"  [{index}] "
            f"type_flag={decoded['type_flag']} "
            f"level={decoded['level']} "
            f"code=0x{decoded['code']:04X} "
            f"high32=0x{item.high32():08X} "
            f"low32=0x{item.low32():08X}"
        )


running = {"value": True}


def _stop(signum, frame):
    running["value"] = False
    print(f"\nsignal {signum} received, stopping ...", flush=True)


# fastdds 的原生信号处理会吞掉 Ctrl+C，必须显式覆盖才能正常退出
signal.signal(signal.SIGINT, _stop)
signal.signal(signal.SIGTERM, _stop)

dds.create_subscription(
    ErrorCodes.ErrorCodesPubSubType,
    "hal_bms_error_code",
    on_error,
    qos_profile=SubscriberQosProfile.default(),
)

try:
    while running["value"]:
        time.sleep(0.1)
finally:
    dds.close()

健康机器人上没有故障码，该脚本不会有任何回调输出，属正常现象；存在故障码时约 1 Hz 持续输出。

4.3 C++ - 订阅电池状态​

C++
#include <BmsState/BmsStatePubSubTypes.hpp>
#include "fourier_dds/dds_node.hpp"
#include "fourier_dds/dds_subscription.hpp"

#include <chrono>
#include <iostream>
#include <memory>
#include <thread>

int main() {
    auto node = std::make_shared<fourier_dds::DdsNode>(123);

    auto bms_sub = std::make_shared<
        fourier_dds::DdsSubscription<fourier_msgs::msg::BmsStatusMsgPubSubType>>(
        node,
        "hal_battery_monitor_status",
        [](const fourier_msgs::msg::BmsStatusMsg& msg) {
            const auto& states = msg.bms_state_list();
            for (size_t i = 0; i < states.size(); ++i) {
                const auto& state = states[i];
                std::cout << "[bms:" << i << "] "
                          << "level=" << static_cast<int>(state.level()) << "% "
                          << "voltage=" << state.voltage() << "V "
                          << "current=" << state.current() << "A "
                          << "failure=" << state.failure()
                          << std::endl;
            }
        });

    while (true) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

编译示例（注意必须链接 fourierdds_core，缺少会报 undefined reference to fourier_dds::...）：

Bash
g++ -std=c++17 watch_grpower_status.cpp -o watch_grpower_status \
  -I/opt/fftai/include/fourier_dds \
  -I/opt/fftai/fourier_dds_msgs/include/fourier_dds_msgs \
  -L/opt/fftai/fourier_dds_msgs/lib \
  -Wl,-rpath,/opt/fftai/fourier_dds_msgs/lib \
  -lBmsState \
  -lfourierdds_core \
  -lfastdds \
  -lfastcdr \
  -pthread

运行示例（普通用户即可）：

Bash
FASTDDS_BUILTIN_TRANSPORTS=UDPv4 ./watch_grpower_status

5. DDS 联调流程​

5.1 观察电池状态​

  1. 确认机器人上 fourierpower 已运行（开机自启）。
  2. 运行状态订阅脚本：
    Bash
    python3 watch_grpower_status.py
  3. 确认订阅端收到 hal_battery_monitor_status：GR-W 为双电池，bms_state_list 应为 2 个元素，battery_comm=true，电压、电量为合理值（电流为负表示放电）。

5.2 观察故障码​

  1. 运行 4.2 节的故障码订阅脚本：
    Bash
    python3 watch_grpower_errors.py
  2. 健康机器人上无输出属正常；存在故障码时会收到 source=4 的 hal_bms_error_code。
  3. 按 type_flag、level、code 和 low32 解读故障来源与具体故障位。

6. 故障排查与注意事项​

收不到电池状态​

确认 fourierpower 已运行（ps -C fourierpower）、DDS domain 为 123，订阅端 topic 名称和消息类型与表格一致，且已设置 FASTDDS_BUILTIN_TRANSPORTS=UDPv4。未设置该变量时会出现端点匹配但收不到数据、或间歇性收到数据的情况；sudo 不是解决收包问题的正确手段。

收不到故障码​

确认订阅端使用 hal_bms_error_code 和 fourier_msgs/msg/ErrorCodes，并且 DDS domain 与 fourierpower 保持一致。ErrorCodes 只有在 GR-Power 存在故障码时才会发布：健康机器人上订阅不到回调属正常，不是故障。

ErrorCodes.source=4​

GR-Power 会将 ErrorCodes.source 设置为 4，表示故障来源是 power / battery 模块。