GR-MotionBank

1. 概述

GR-MotionBank 负责播放动作库中的预编排动作。用户进程通过 DDS 发送动作编号，运行时从本地动作库加载对应动作，并回报 MotionBank 运行状态。

元素	数值
Domain ID	113
命令 topic	algorithm_cmd
状态 topic	algorithm_state
默认动作ID示例	6008

2. DDS 接口

2.1 Topics

订阅

Topic	消息类型	说明
algorithm_cmd	fourier_msgs/msg/AlgorithmCmd	动作播放 / 控制命令

发布

Topic	消息类型	说明
algorithm_state	fourier_msgs/msg/AlgorithmState	MotionBank 运行状态

2.2 消息字段

AlgorithmCmd

MotionBank 当前只解析 input_args[0]：

字段	用途
name	固定填 "MotionBank"
cmd_state	播放动作时填 1
group_mask	控制组掩码，常用 0xFF
input_args[0].arg_int	动作编号队列，例如 [6008]
input_args[0].arg_string[0]	可选控制字：stop、break、continue、reset

AlgorithmState

字段	用途
name	MotionBank 状态固定为 "motionBank"
running_state	1 表示动作生命周期中，0 表示停止

3. 支持动作列表

当前生效动作库支持以下动作编号：

动作编号	动作名称
6001	打招呼
6002	抬手挥手
6005	点赞
6007	右边请
6008	左边请
6013	左比心
6014	右比心
6015	比耶
6023	鼓掌
6026	拍照
6030	碰拳
6031	加油
6032	拥抱
6033	右手握手
6034	Skr
6036	双手比心
6101	礼仪姿势
6103	摊手说明
6104	左手重点
6105	右手重点
6106	健身操
6143	随机说明动作1
6144	随机说明动作2
6145	随机展示动作
6146	随机动作测试
6147	双手打叉
6148	击掌
6149	敬礼
6150	举手
6151	双手平举

4. 基础示例

4.1 Python —— 播放动作 6008

下面脚本会发布播放命令，机器人会执行动作。运行前确认机器人周围安全，并确认 motionbank 正在运行。

Python

import time
from threading import Event

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

MOTION_ID = 6008
DOMAIN_ID = 113

dds = DDSInterface(domain_id=DOMAIN_ID)

cmd_pub = dds.create_publisher(
    AlgorithmCmd.AlgorithmCmdPubSubType,
    "algorithm_cmd",
)

started = Event()
finished = Event()
published = False
last_state = None


def on_state(msg):
    global last_state

    if msg.name() != "motionBank":
        return

    running = int(msg.running_state())
    if running != last_state:
        print(f"[state] motionBank running_state={running}")
        last_state = running

    if not published:
        return
    if running == 1:
        started.set()
    elif started.is_set() and running == 0:
        finished.set()


state_sub = dds.create_subscription(
    AlgorithmCmd.AlgorithmStatePubSubType,
    "algorithm_state",
    on_state,
    qos_profile=SubscriberQosProfile.default(),
)

# 等待 DDS 发现完成。
time.sleep(3.0)

base = AlgorithmCmd.BaseDataType()
base.arg_int([MOTION_ID])

cmd = AlgorithmCmd.AlgorithmCmd()
cmd.name("MotionBank")
cmd.cmd_state(1)
cmd.group_mask(0xFF)
cmd.input_args([base])

published = True
print(f"[publish] motion_id={MOTION_ID}")
cmd_pub.publish(cmd)

if not started.wait(5.0):
    raise RuntimeError(f"motion {MOTION_ID} did not enter running")
print(f"motion {MOTION_ID} entered running")

if not finished.wait(60.0):
    raise RuntimeError(f"motion {MOTION_ID} did not finish")
print(f"motion {MOTION_ID} finished")

dds.close()

本机运行示例建议使用 sudo 并继承环境变量；否则可能动作已下发执行，但收不到 root 进程发布的 algorithm_state 回调，导致示例误报超时。

Bash

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

4.2 C++ —— 播放动作 6008

下面示例同样会发布播放命令，机器人会执行动作。运行前确认机器人周围安全，并确认 motionbank 正在运行。

C++

#include <AlgorithmCmd/AlgorithmCmdPubSubTypes.hpp>
#include <AlgorithmState/AlgorithmStatePubSubTypes.hpp>
#include "fourier_dds/dds_node.hpp"
#include "fourier_dds/dds_publisher.hpp"
#include "fourier_dds/dds_subscription.hpp"

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

int main() {
    constexpr int32_t kMotionId = 6008;

    std::atomic<bool> published{false};
    std::atomic<bool> started{false};
    std::atomic<bool> finished{false};
    std::atomic<int> last_state{-1};

    auto node = std::make_shared<fourier_dds::DdsNode>(113);
    auto cmd_pub = std::make_shared<
        fourier_dds::DdsPublisher<fourier_msgs::msg::AlgorithmCmdPubSubType>>(
        node,
        "algorithm_cmd");

    auto state_sub = std::make_shared<
        fourier_dds::DdsSubscription<fourier_msgs::msg::AlgorithmStatePubSubType>>(
        node,
        "algorithm_state",
        [&](const fourier_msgs::msg::AlgorithmState& msg) {
            if (msg.name() != "motionBank") {
                return;
            }

            const int running = static_cast<int>(msg.running_state());
            const int previous = last_state.exchange(running);
            if (running != previous) {
                std::cout << "[state] motionBank running_state=" << running << std::endl;
            }

            if (!published.load()) {
                return;
            }
            if (running == 1) {
                started = true;
            } else if (started.load() && running == 0) {
                finished = true;
            }
        });

    std::this_thread::sleep_for(std::chrono::seconds(3));

    std_msgs::msg::BaseDataType base;
    base.arg_int(std::vector<int32_t>{kMotionId});

    fourier_msgs::msg::AlgorithmCmd cmd;
    cmd.name("MotionBank");
    cmd.cmd_state(1);
    cmd.group_mask(0xFF);
    cmd.input_args(std::vector<std_msgs::msg::BaseDataType>{base});

    published = true;
    std::cout << "[publish] motion_id=" << kMotionId << std::endl;
    if (!cmd_pub->publish(cmd)) {
        std::cerr << "failed to publish motion command" << std::endl;
        return 1;
    }

    const auto enter_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
    while (!started.load() && std::chrono::steady_clock::now() < enter_deadline) {
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    if (!started.load()) {
        std::cerr << "motion " << kMotionId << " did not enter running" << std::endl;
        return 2;
    }
    std::cout << "motion " << kMotionId << " entered running" << std::endl;

    const auto finish_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(60);
    while (!finished.load() && std::chrono::steady_clock::now() < finish_deadline) {
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    if (!finished.load()) {
        std::cerr << "motion " << kMotionId << " did not finish" << std::endl;
        return 3;
    }
    std::cout << "motion " << kMotionId << " finished" << std::endl;
    return 0;
}

编译示例：

Bash

g++ -std=c++17 play_motionbank_6008.cpp -o play_motionbank_6008 \
  -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 \
  -lAlgorithmCmd \
  -lAlgorithmState \
  -lfastdds \
  -lfastcdr \
  -pthread

C++ 可执行程序本机运行时同样建议使用 sudo 并继承环境变量。

Bash

sudo env "PYTHONPATH=$PYTHONPATH" "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" ./play_motionbank_6008

5. 故障排查与注意事项

没有进入 running

确认 motionbank 进程正在运行，订阅端与发布端都使用 domain 113，并且 topic 为 algorithm_cmd / algorithm_state。

消息类型必须匹配

algorithm_cmd 使用 fourier_msgs/msg/AlgorithmCmd，algorithm_state 使用 fourier_msgs/msg/AlgorithmState。Python 绑定中两个 PubSubType 都从 fourier_msgs.msg.AlgorithmCmd 模块导入。

动作编号

动作编号会映射到 MotionBank 动作库中的 motion_<id>/config_motion.json 和 arms/arm<id>/left_arm.txt / right_arm.txt。示例使用 6008。