跳到主要内容

GR-Power

1. 概述​

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

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

2. DDS 接口​

2.1 发布 Topics​

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

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] 表示副电池。

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 电池故障

4. 基础示例​

4.1 Python - 订阅电池状态​

Python
import time

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()}"
        )


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

while True:
    time.sleep(0.1)

运行示例：

Bash
python3 watch_grpower_status.py

如果运行时服务由 root 启动，订阅端 matched 但收不到 callback，可以保留 DDS 环境变量后用 sudo 运行：

Bash
sudo env "PYTHONPATH=$PYTHONPATH" "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" python3 -u watch_grpower_status.py

4.2 Python - 订阅故障码​

Python
import time

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}"
        )


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

while True:
    time.sleep(0.1)

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));
    }
}

编译示例：

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 \
  -lfastdds \
  -lfastcdr \
  -pthread

运行示例：

Bash
sudo env "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" ./watch_grpower_status

5. DDS 联调流程​

5.1 观察电池状态​

  1. 在机器人上启动 Fourierpower。
  2. 运行状态订阅脚本：
    Bash
    python3 watch_grpower_status.py
  3. 确认订阅端收到 hal_battery_monitor_status，并且 bms_state_list 中的电压、电流、电量和通信状态符合预期。

5.2 观察故障码​

  1. 启动故障码订阅脚本：
    Bash
    python3 watch_grpower_errors.py
  2. 确认订阅端收到 source=4 的 hal_bms_error_code。
  3. 按 type_flag、level、code 和 low32 解读故障来源与具体故障位。

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

收不到电池状态​

确认 Fourierpower 已启动、DDS domain 为 123，订阅端使用的 topic 名称和消息类型与表格一致。若订阅端 matched 但没有 callback，优先检查 DDS 环境变量、运行用户和动态库路径。

收不到故障码​

确认订阅端使用 hal_bms_error_code 和 fourier_msgs/msg/ErrorCodes，并且 DDS domain 与 Fourierpower 保持一致。ErrorCodes 只有在 GR-Power 发布故障时才会更新。

ErrorCodes.source=4​

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