Odometry

数据结构

字节索引	类型	字段含义
0-7	float64	X 轴位置
8-15	float64	Y 轴位置
16-23	float64	Z 轴位置
24-31	float64	X 轴姿态四元数
32-39	float64	Y 轴姿态四元数
40-47	float64	Z 轴姿态四元数
48-55	float64	W 轴姿态四元数
56-63	float64	X 轴线速度
64-71	float64	Y 轴线速度
72-79	float64	Z 轴线速度
80-87	float64	X 轴角速度
88-95	float64	Y 轴角速度
96-103	float64	Z 轴角速度

Odometry 固定为 104 字节

示例

C++ 示例

struct Vector3 {
  double x;
  double y;
  double z;
};

struct Quaternion {
  double x;
  double y;
  double z;
  double w;
};

struct Pose {
  Vector3 position;
  Quaternion orientation;
};

struct Twist {
  Vector3 linear;
  Vector3 angular;
};

struct Odometry {
  Pose pose;
  Twist twist;
};

Rust 示例

#[repr(C)]
#[derive(Clone, Copy, Debug, Default)]
pub struct Vector3 {
    pub x: f64,
    pub y: f64,
    pub z: f64,
}

#[repr(C)]
#[derive(Clone, Copy, Debug, Default)]
pub struct Quaternion {
    pub x: f64,
    pub y: f64,
    pub z: f64,
    pub w: f64,
}

#[repr(C)]
#[derive(Clone, Copy, Debug, Default)]
pub struct Pose {
    pub position: Vector3,
    pub orientation: Quaternion,
}

#[repr(C)]
#[derive(Clone, Copy, Debug, Default)]
pub struct Twist {
    pub linear: Vector3,
    pub angular: Vector3,
}

#[repr(C)]
#[derive(Clone, Copy, Debug, Default)]
pub struct Odometry {
    pub pose: Pose,
    pub twist: Twist,
}

const _: () = {
    assert!(core::mem::size_of::<Vector3>() == 24);
    assert!(core::mem::size_of::<Quaternion>() == 32);
    assert!(core::mem::size_of::<Pose>() == 56);
    assert!(core::mem::size_of::<Twist>() == 48);
    assert!(core::mem::size_of::<Odometry>() == 104);
    assert!(core::mem::align_of::<Odometry>() == 8);
};

Dart 示例

import 'dart:typed_data';

final class OdometryView {
  static const int byteLength = 104;

  final ByteData data;

  OdometryView(ByteBuffer buffer, [int offsetInBytes = 0])
      : data = ByteData.view(buffer, offsetInBytes, byteLength);

  double get positionX => data.getFloat64(0, Endian.little);
  set positionX(double value) => data.setFloat64(0, value, Endian.little);

  double get positionY => data.getFloat64(8, Endian.little);
  set positionY(double value) => data.setFloat64(8, value, Endian.little);

  double get positionZ => data.getFloat64(16, Endian.little);
  set positionZ(double value) => data.setFloat64(16, value, Endian.little);

  double get orientationX => data.getFloat64(24, Endian.little);
  set orientationX(double value) => data.setFloat64(24, value, Endian.little);

  double get orientationY => data.getFloat64(32, Endian.little);
  set orientationY(double value) => data.setFloat64(32, value, Endian.little);

  double get orientationZ => data.getFloat64(40, Endian.little);
  set orientationZ(double value) => data.setFloat64(40, value, Endian.little);

  double get orientationW => data.getFloat64(48, Endian.little);
  set orientationW(double value) => data.setFloat64(48, value, Endian.little);

  double get linearX => data.getFloat64(56, Endian.little);
  set linearX(double value) => data.setFloat64(56, value, Endian.little);

  double get linearY => data.getFloat64(64, Endian.little);
  set linearY(double value) => data.setFloat64(64, value, Endian.little);

  double get linearZ => data.getFloat64(72, Endian.little);
  set linearZ(double value) => data.setFloat64(72, value, Endian.little);

  double get angularX => data.getFloat64(80, Endian.little);
  set angularX(double value) => data.setFloat64(80, value, Endian.little);

  double get angularY => data.getFloat64(88, Endian.little);
  set angularY(double value) => data.setFloat64(88, value, Endian.little);

  double get angularZ => data.getFloat64(96, Endian.little);
  set angularZ(double value) => data.setFloat64(96, value, Endian.little);
}

Java 示例

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.DoubleBuffer;

public final class OdometryView {
  public static final int BYTE_LENGTH = Double.BYTES * 13;

  private final DoubleBuffer values;

  public OdometryView(ByteBuffer buffer, int offset) {
    if (offset < 0 || offset + BYTE_LENGTH > buffer.capacity()) {
      throw new IndexOutOfBoundsException("Odometry payload requires 104 bytes");
    }
    this.values = buffer.order(ByteOrder.LITTLE_ENDIAN)
        .slice(offset, BYTE_LENGTH)
        .asDoubleBuffer();
  }

  public double positionX() { return values.get(0); }
  public void positionX(double value) { values.put(0, value); }

  public double positionY() { return values.get(1); }
  public void positionY(double value) { values.put(1, value); }

  public double positionZ() { return values.get(2); }
  public void positionZ(double value) { values.put(2, value); }

  public double orientationX() { return values.get(3); }
  public void orientationX(double value) { values.put(3, value); }

  public double orientationY() { return values.get(4); }
  public void orientationY(double value) { values.put(4, value); }

  public double orientationZ() { return values.get(5); }
  public void orientationZ(double value) { values.put(5, value); }

  public double orientationW() { return values.get(6); }
  public void orientationW(double value) { values.put(6, value); }

  public double linearX() { return values.get(7); }
  public void linearX(double value) { values.put(7, value); }

  public double linearY() { return values.get(8); }
  public void linearY(double value) { values.put(8, value); }

  public double linearZ() { return values.get(9); }
  public void linearZ(double value) { values.put(9, value); }

  public double angularX() { return values.get(10); }
  public void angularX(double value) { values.put(10, value); }

  public double angularY() { return values.get(11); }
  public void angularY(double value) { values.put(11, value); }

  public double angularZ() { return values.get(12); }
  public void angularZ(double value) { values.put(12, value); }
}