Skip to main content

GR-Sensor

1. Overview​

GR-Sensor is the sensor-data publishing layer. The runtime service fouriersensor reads orientation, angular velocity, and linear acceleration from the body IMU. It can also acquire RGB images, depth images, and camera intrinsics from the head-mounted Orbbec Gemini 335L and publish them over DDS. This page documents the currently available GRW IMU and camera topic contracts.

ElementValue
Domain ID123
Hardware accessfouriersensor runs as root to access device files; subscribers run as regular users
Current supportIMU and head RGB-D camera
IMU topicImu_state
Camera topic prefix/camera/head_downward
Runtime environment

fouriersensor runs as a system user on the robot. Subscription scripts just need to run as a regular user — no root/sudo required — but you must set the following before creating any DDS participant:

Bash
export FASTDDS_BUILTIN_TRANSPORTS=UDPv4

Otherwise Fast DDS may pick the shared-memory transport on the same host and you will see matched endpoints but no incoming data — or data arriving only intermittently. The Python examples below already embed this setting.

2. DDS Interface​

2.1 Topics​

IMU​

TopicMessage TypeDescription
Imu_statefourier_msgs_pod/msg/ImuBody IMU, default topic

Head RGB-D Camera​

TopicMessage TypeDescription
/camera/head_downward/rgb/image_compressedsensor_msgs_pod/msg/ImagePod3mRGB JPEG image, published by default
/camera/head_downward/depth/image_rawsensor_msgs_pod/msg/ImagePod3mDepth image aligned to RGB coordinates; default encoding is 16UC1 in millimeters
/camera/head_downward/rgb/camera_infosensor_msgs/msg/CameraInfoRGB camera intrinsics and distortion parameters
/camera/head_downward/depth/camera_infosensor_msgs/msg/CameraInfoDepth camera intrinsics; when depth-to-RGB alignment is enabled, this uses the RGB coordinate system
/camera/head_downward/rgb/image_rawsensor_msgs_pod/msg/ImagePod3mUncompressed RGB image, published only when publish_rgb_raw is enabled

Camera images are high-bandwidth real-time streams, so subscribers should use Best Effort QoS; under Best Effort the received rate of large streams is lower than the publish rate (e.g. a depth stream published at 10 Hz arrives at roughly 4 Hz) — this is normal, do not use receive counts as a publish-rate criterion. ImagePod3m.data is a fixed-capacity array; use data_size as the valid payload length. The Python binding does not support partial slicing of data (only a full slice or per-index element access); access pixels by index up to data_size.

2.2 IDL Definitions​

Imu​

Idl
// fourier_msgs_pod/msg/Imu — body 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;                  // robot yaw-direction bias vector
};

ImagePod3m​

Idl
// sensor_msgs_pod/msg/ImagePod3m — RGB or depth image
typedef uint8 ImagePod3mData__3145728[3145728];

struct ImagePod3m {
    HeaderPod header;
    uint32 height;
    uint32 width;
    ShortString encoding;                 // for example: jpeg, bgr8, 16UC1
    boolean is_bigendian;
    uint32 step;                          // bytes per row
    ImagePod3mData__3145728 data;
    uint32 data_size;                     // valid byte count in data
};

CameraInfo​

Idl
// sensor_msgs/msg/CameraInfo — fields shared by all supported versions
struct CameraInfo {
    Header header;
    uint32 height;
    uint32 width;
    string distortion_model;
    sequence<double> d;                   // distortion parameters
    double k[9];                          // 3x3 intrinsic matrix
    double r[9];                          // 3x3 rectification matrix
    double p[12];                         // 3x4 projection matrix
    uint32 binning_x;
    uint32 binning_y;
    RegionOfInterest roi;
};

Some newer message versions also include camera model, serial number, and firmware version fields. For compatibility across software versions, the examples below access only the shared fields.

3. Basic Examples​

3.1 Python — Subscribe to IMU​

Python
import os
import signal
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")  # required on the robot

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 installs a native signal handler that swallows Ctrl+C;
# override it explicitly so the script can exit normally
signal.signal(signal.SIGINT, _stop)
signal.signal(signal.SIGTERM, _stop)

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

3.2 Python — Subscribe to RGB, Depth, and CameraInfo​

Python
import os
import signal
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")  # required on the robot

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 installs a native signal handler that swallows Ctrl+C;
# override it explicitly so the script can exit normally
signal.signal(signal.SIGINT, _stop)
signal.signal(signal.SIGTERM, _stop)

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

3.3 C++ — Subscribe to 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));
    }
}

Build example:

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

Run example (regular user is sufficient; C++ has no PYTHONPATH dependency):

Bash
FASTDDS_BUILTIN_TRANSPORTS=UDPv4 ./imu_sub

3.4 C++ — Subscribe to RGB, Depth, and 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));
    }
}

Build example:

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

Run example (regular user is sufficient):

Bash
FASTDDS_BUILTIN_TRANSPORTS=UDPv4 ./camera_sub

4. Camera Enablement and Verification​

The GRW sensor configuration is located at:

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

Use cfg_charming.json in the same directory as a reference when enabling the Orbbec camera. The following 640×480, 10 Hz settings have been validated end to end:

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 must be the value enumerated by the local SDK. Restart fouriersensor after editing the configuration, then run the example from Section 3.2 or 3.4. All four callbacks should receive continuously at about 10 Hz.

5. Troubleshooting & Reminders​

Permissions​

Subscription scripts run as a regular user — no sudo needed; fouriersensor is started by the system as root to access device files, which is unrelated to subscribers. The key to cross-user communication is the UDP transport prerequisite:

Bash
FASTDDS_BUILTIN_TRANSPORTS=UDPv4 python3 -u camera_sub.py

Without this variable, endpoints may match but deliver no data — or deliver it only intermittently; sudo is not the right fix for delivery problems.

No Data​

Confirm that fouriersensor is running, Domain ID is 123, and topic names match their message types. For camera data, also confirm that the Orbbec is connected, head_downward is enabled, usb_port_id is correct, and image subscriptions use Best Effort QoS. Process only the first data_size bytes of ImagePod3m.data (the Python binding does not support partial slicing of data; access by index).