GR-Sensor

1. 概述

GR-Sensor 是传感器数据发布层。运行时服务 fouriersensor 从机身 IMU 读取姿态、角速度和线加速度，也可从头部 Orbbec Gemini 335L 获取 RGB 图像、深度图像及相机内参，并通过 DDS 发布。本页描述 GRW 当前可用的 IMU 与相机 topic 协议。

元素	数值
Domain ID	123
硬件访问	fouriersensor 运行时以 root 访问设备文件；订阅端普通用户即可
当前支持	IMU、头部 RGB-D 相机
IMU Topic	Imu_state
相机 Topic 前缀	/camera/head_downward

运行环境

fouriersensor 在机器人上以系统用户运行。二开订阅脚本以普通用户运行即可，无需 root/sudo；但必须在创建任何 DDS participant 前设置：

Bash

export FASTDDS_BUILTIN_TRANSPORTS=UDPv4

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

2. DDS 接口

2.1 Topics

IMU

Topic	消息类型	说明
Imu_state	fourier_msgs_pod/msg/Imu	机身 IMU，默认 topic

头部 RGB-D 相机

Topic	消息类型	说明
/camera/head_downward/rgb/image_compressed	sensor_msgs_pod/msg/ImagePod3m	RGB JPEG 图像，默认发布
/camera/head_downward/depth/image_raw	sensor_msgs_pod/msg/ImagePod3m	对齐到 RGB 坐标系的深度图，默认编码为 16UC1，单位为毫米
/camera/head_downward/rgb/camera_info	sensor_msgs/msg/CameraInfo	RGB 相机内参及畸变参数
/camera/head_downward/depth/camera_info	sensor_msgs/msg/CameraInfo	深度相机内参；启用深度到 RGB 对齐后与 RGB 图像使用相同坐标系
/camera/head_downward/rgb/image_raw	sensor_msgs_pod/msg/ImagePod3m	未压缩 RGB 图像，仅在 publish_rgb_raw 启用时发布

相机图像是大数据量实时流，订阅端应使用 Best Effort QoS；大数据流在 Best Effort 下的接收速率会低于发布速率（例如 10 Hz 发布的深度流接收约 4 Hz），属正常现象，不要用接收计数当作发布频率判据。ImagePod3m.data 是固定容量数组，实际有效长度以 data_size 为准；Python 绑定不支持对 data 做部分切片（只能整段切片或按索引逐元素读取），处理像素时应按 data_size 用索引访问。

2.2 IDL 定义

Imu

Idl

// fourier_msgs_pod/msg/Imu —— 机身 IMU
struct Imu {
    HeaderPod header;
    TracePointList trace_points;
    Quaternion orientation;               // x, y, z, w
    double orientation_covariance[9];
    Vector3 angular_velocity;             // rad/s
    double angular_velocity_covariance[9];
    Vector3 linear_acceleration;          // m/s^2
    double linear_acceleration_covariance[9];
    Vector3 bias_vector;                  // 机器人 yaw 方向偏置向量
};

ImagePod3m

Idl

// sensor_msgs_pod/msg/ImagePod3m —— RGB 或深度图像
typedef uint8 ImagePod3mData__3145728[3145728];

struct ImagePod3m {
    HeaderPod header;
    uint32 height;
    uint32 width;
    ShortString encoding;                 // 例如 jpeg、bgr8、16UC1
    boolean is_bigendian;
    uint32 step;                          // 每行字节数
    ImagePod3mData__3145728 data;
    uint32 data_size;                     // data 中的实际有效字节数
};

CameraInfo

Idl

// sensor_msgs/msg/CameraInfo —— 下列为各版本共有字段
struct CameraInfo {
    Header header;
    uint32 height;
    uint32 width;
    string distortion_model;
    sequence<double> d;                   // 畸变参数
    double k[9];                          // 3x3 内参矩阵
    double r[9];                          // 3x3 校正矩阵
    double p[12];                         // 3x4 投影矩阵
    uint32 binning_x;
    uint32 binning_y;
    RegionOfInterest roi;
};

部分较新的消息版本还包含相机型号、序列号和固件版本字段。为了兼容不同软件版本，下面的示例只访问共有字段。

3. 基础示例

3.1 Python —— 订阅 IMU

Python

import os
import signal
import time

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

from fourierdds_py import DDSInterface, SubscriberQosProfile
import fourier_msgs_pod.msg.ImuPod as ImuPod

dds = DDSInterface(domain_id=123)

def on_imu(msg):
    q = msg.orientation()
    w = msg.angular_velocity()
    a = msg.linear_acceleration()
    b = msg.bias_vector()
    print(
        "[imu]\n"
        f"  orientation xyzw: [{q.x():.6f}, {q.y():.6f}, {q.z():.6f}, {q.w():.6f}]\n"
        f"  angular_velocity rad/s: [{w.x():.6f}, {w.y():.6f}, {w.z():.6f}]\n"
        f"  linear_acceleration m/s^2: [{a.x():.6f}, {a.y():.6f}, {a.z():.6f}]\n"
        f"  bias_vector: [{b.x():.6f}, {b.y():.6f}, {b.z():.6f}]"
    )

dds.create_subscription(
    ImuPod.ImuPubSubType,
    "Imu_state",
    on_imu,
    qos_profile=SubscriberQosProfile.default(),
)

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)

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

3.2 Python —— 订阅 RGB、深度图和 CameraInfo

Python

import os
import signal
import time

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

from fourierdds_py import DDSInterface, SubscriberQosProfile
from sensor_msgs.msg.CameraInfo import CameraInfoPubSubType
import sensor_msgs_pod.msg.ImagePod3m as ImagePod3m

dds = DDSInterface(domain_id=123)
qos = SubscriberQosProfile.best_effort()

def short_string(value):
    return "".join(chr(c) for c in value.content() if c)

def on_image(name, msg):
    print(
        f"[{name}] {msg.width()}x{msg.height()} "
        f"encoding={short_string(msg.encoding())} "
        f"step={msg.step()} bytes={msg.data_size()}"
    )

def on_camera_info(name, msg):
    k = msg.k()
    print(
        f"[{name}] {msg.width()}x{msg.height()} "
        f"model={msg.distortion_model()} "
        f"fx={k[0]:.6f} fy={k[4]:.6f} "
        f"cx={k[2]:.6f} cy={k[5]:.6f}"
    )

subscriptions = [
    dds.create_subscription(
        ImagePod3m.ImagePod3mPubSubType,
        "/camera/head_downward/rgb/image_compressed",
        lambda msg: on_image("rgb", msg),
        qos_profile=qos,
    ),
    dds.create_subscription(
        ImagePod3m.ImagePod3mPubSubType,
        "/camera/head_downward/depth/image_raw",
        lambda msg: on_image("depth", msg),
        qos_profile=qos,
    ),
    dds.create_subscription(
        CameraInfoPubSubType,
        "/camera/head_downward/rgb/camera_info",
        lambda msg: on_camera_info("rgb_camera_info", msg),
        qos_profile=qos,
    ),
    dds.create_subscription(
        CameraInfoPubSubType,
        "/camera/head_downward/depth/camera_info",
        lambda msg: on_camera_info("depth_camera_info", msg),
        qos_profile=qos,
    ),
]

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)

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

3.3 C++ —— 订阅 IMU

C++

#include <ImuPod/ImuPodPubSubTypes.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 sub = std::make_shared<
        fourier_dds::DdsSubscription<fourier_msgs_pod::msg::ImuPubSubType>>(
        node,
        "Imu_state",
        [](const fourier_msgs_pod::msg::Imu& msg) {
            std::cout << "[imu]\n"
                      << "  orientation xyzw: ["
                      << msg.orientation().x() << ", "
                      << msg.orientation().y() << ", "
                      << msg.orientation().z() << ", "
                      << msg.orientation().w() << "]\n"
                      << "  angular_velocity rad/s: ["
                      << msg.angular_velocity().x() << ", "
                      << msg.angular_velocity().y() << ", "
                      << msg.angular_velocity().z() << "]\n"
                      << "  linear_acceleration m/s^2: ["
                      << msg.linear_acceleration().x() << ", "
                      << msg.linear_acceleration().y() << ", "
                      << msg.linear_acceleration().z() << "]\n"
                      << "  bias_vector: ["
                      << msg.bias_vector().x() << ", "
                      << msg.bias_vector().y() << ", "
                      << msg.bias_vector().z() << "]\n";
        });

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

编译示例：

Bash

g++ -std=c++17 imu_sub.cpp -o imu_sub \
  -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 \
  -lImuPod \
  -lfourierdds_core \
  -lfastdds \
  -lfastcdr \
  -pthread

运行示例（普通用户即可，C++ 无 PYTHONPATH 依赖）：

Bash

FASTDDS_BUILTIN_TRANSPORTS=UDPv4 ./imu_sub

3.4 C++ —— 订阅 RGB、深度图和 CameraInfo

C++

#include <CameraInfo/CameraInfoPubSubTypes.hpp>
#include <ImagePod3m/ImagePod3mPubSubTypes.hpp>
#include "fourier_dds/dds_node.hpp"
#include "fourier_dds/dds_subscription.hpp"

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

void PrintImage(
        const char* name,
        const sensor_msgs_pod::msg::ImagePod3m& msg) {
    std::cout << "[" << name << "] "
              << msg.width() << "x" << msg.height()
              << " step=" << msg.step()
              << " bytes=" << msg.data_size() << "\n";
}

void PrintCameraInfo(
        const char* name,
        const sensor_msgs::msg::CameraInfo& msg) {
    const auto& k = msg.k();
    std::cout << "[" << name << "] "
              << msg.width() << "x" << msg.height()
              << " model=" << msg.distortion_model()
              << " fx=" << k[0] << " fy=" << k[4]
              << " cx=" << k[2] << " cy=" << k[5] << "\n";
}

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

    using ImageSub = fourier_dds::DdsSubscription<
        sensor_msgs_pod::msg::ImagePod3mPubSubType>;
    using CameraInfoSub = fourier_dds::DdsSubscription<
        sensor_msgs::msg::CameraInfoPubSubType>;

    auto rgb_sub = std::make_shared<ImageSub>(
        node,
        "/camera/head_downward/rgb/image_compressed",
        [](const sensor_msgs_pod::msg::ImagePod3m& msg) {
            PrintImage("rgb", msg);
        });

    auto depth_sub = std::make_shared<ImageSub>(
        node,
        "/camera/head_downward/depth/image_raw",
        [](const sensor_msgs_pod::msg::ImagePod3m& msg) {
            PrintImage("depth", msg);
        });

    auto rgb_info_sub = std::make_shared<CameraInfoSub>(
        node,
        "/camera/head_downward/rgb/camera_info",
        [](const sensor_msgs::msg::CameraInfo& msg) {
            PrintCameraInfo("rgb_camera_info", msg);
        });

    auto depth_info_sub = std::make_shared<CameraInfoSub>(
        node,
        "/camera/head_downward/depth/camera_info",
        [](const sensor_msgs::msg::CameraInfo& msg) {
            PrintCameraInfo("depth_camera_info", msg);
        });

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

编译示例：

Bash

g++ -std=c++17 camera_sub.cpp -o camera_sub \
  -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 \
  -lImagePod3m \
  -lCameraInfo \
  -lfourierdds_core \
  -lfastdds \
  -lfastcdr \
  -pthread

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

Bash

FASTDDS_BUILTIN_TRANSPORTS=UDPv4 ./camera_sub

4. 相机启用与验证

GRW 的传感器配置位于：

Text

/opt/fftai/fourier_config_center/function_config/sensor/cfg_grw.json

参考同目录的 cfg_charming.json 启用 Orbbec。以下是已闭环验证的 640×480、10 Hz 关键配置：

JSON

{
  "sensor_enable": {"camera": {"orbbec": {"head_downward": true}}},
  "camera_orbbec_config": {"cameras": [{
    "role": "head_downward",
    "publish_rgb_compressed": true,
    "publish_depth_raw": true,
    "rgb_width": 640,
    "rgb_height": 480,
    "rgb_frequence": 10,
    "depth_width": 640,
    "depth_height": 480,
    "depth_frequence": 10
  }]}
}

usb_port_id 必须使用本机 SDK 枚举值。修改后重启 fouriersensor，再运行 3.2 或 3.4 节示例；四个回调应持续收到数据，发布频率约为 10 Hz。

5. 故障排查与注意事项

运行权限

订阅脚本以普通用户运行即可，无需 sudo；fouriersensor 由系统以 root 拉起并访问设备文件，与订阅端无关。跨用户通信的关键是 UDP transport 前置：

Bash

FASTDDS_BUILTIN_TRANSPORTS=UDPv4 python3 -u camera_sub.py

未设置该变量时会出现端点匹配但收不到数据（或间歇性收到）的情况；sudo 不是解决收包问题的正确手段。

收不到数据

确认 fouriersensor 正常运行、Domain ID 为 123，并检查 topic 与消息类型是否匹配。相机还需确认 Orbbec 已连接、head_downward 已启用、usb_port_id 正确，图像订阅使用 Best Effort QoS。处理图像时只读取 ImagePod3m.data 的前 data_size 个字节（Python 绑定不支持对 data 部分切片，按索引访问）。