GR-Sensor

1. 概述

GR-Sensor 是传感器数据订阅层。运行时服务 grsensor 从机身 IMU 读取姿态、角速度和线加速度，并将触摸控制器的 UDP 数据转换为 DDS 触摸状态。本页描述当前可用的 IMU 与触摸 topic 协议。

🚧 雷达与相机数据，接口建设中

元素	数值
Domain ID	123
硬件访问	运行时需要 root 访问设备文件
当前支持	IMU 数据获取、机身触摸状态
Topic	Imu_state、body_sensor_state

2. DDS 接口

2.1 Topics

IMU

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

触摸

Topic	消息类型	说明
body_sensor_state	fourier_msgs/msg/SensorState	机身触摸状态，非零 bitmask 表示有触摸

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 方向偏置向量
};

SensorState（触摸）

body_sensor_state 中 sensor_name 为 "touch"，sensor_state[0].arg_int[0] 为 16 位触摸 bitmask；只判断是否触摸时，检查该值是否非零即可。

Idl

struct BaseDataType {
    sequence<int32> arg_int;
    sequence<double> arg_double;
    sequence<string> arg_string;
};

struct SensorState {
    Header header;
    string sensor_name;                 // "touch"
    sequence<BaseDataType> sensor_state;
};

3. 基础示例

3.1 Python —— 订阅 IMU

Python

import time
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(),
)

while True:
    time.sleep(0.1)

3.2 Python —— 订阅触摸状态

Python

import time
from fourierdds_py import DDSInterface, SubscriberQosProfile
import fourier_msgs.msg.SensorState as SensorState

dds = DDSInterface(domain_id=123)

def on_touch(msg):
    states = msg.sensor_state()
    if states.size() == 0:
        return

    values = states[0].arg_int()
    if values.size() == 0:
        return

    if values[0] != 0:
        print("[touch] touched")

dds.create_subscription(
    SensorState.SensorStatePubSubType,
    "body_sensor_state",
    on_touch,
    qos_profile=SubscriberQosProfile.default(),
)

while True:
    time.sleep(0.1)

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

运行示例：

Bash

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

3.4 C++ —— 订阅触摸状态

C++

#include <SensorState/SensorStatePubSubTypes.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::msg::SensorStatePubSubType>>(
        node,
        "body_sensor_state",
        [](const fourier_msgs::msg::SensorState& msg) {
            if (msg.sensor_state().empty() || msg.sensor_state()[0].arg_int().empty()) {
                return;
            }

            if (msg.sensor_state()[0].arg_int()[0] != 0) {
                std::cout << "[touch] touched\n";
            }
        });

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

编译示例：

Bash

g++ -std=c++17 touch_sub.cpp -o touch_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 \
  -lSensorState \
  -lfastdds \
  -lfastcdr \
  -pthread

运行示例：

Bash

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

4. 故障排查与注意事项

本机订阅示例权限

grsensor 通常以 root 运行。本机订阅示例也用 root 启动；Python 订阅脚本需要继承 PYTHONPATH、LD_LIBRARY_PATH，否则可能已 matched 但没有 callback。

Bash

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

非登录 SSH 命令先加载登录环境：

Bash

bash -lc 'cd ~/sensor_test && sudo env PYTHONPATH="$PYTHONPATH" LD_LIBRARY_PATH="$LD_LIBRARY_PATH" python3 -u touch.py'

收不到 IMU 数据

先确认 grsensor 正在运行，再检查订阅端是否使用 domain 123、topic Imu_state，以及对应的消息类型是否匹配。

收不到触摸状态

先确认 CommNodeUdpTouch 与 CommNodeDdsSensorState 已启用。触摸控制器通过 UDP 发送到 grsensor:50001，grsensor 收到后发布 DDS topic body_sensor_state。

Topic 与消息类型必须匹配

Imu_state 对应 fourier_msgs_pod/msg/Imu，body_sensor_state 对应 fourier_msgs/msg/SensorState。topic 名称和消息类型不匹配时，DDS 发现可能成功但回调不会收到有效数据。

Domain ID

所有 grsensor topic 都在 domain 123。如一端修改了 domain，另一端也要同步。