Sensor Interface

Sensor Interface provides telemetry required by developers and researchers to build custom applications and advanced control algorithms. This interface streams high-frequency diagnostic and kinematic data directly from the Svan M2's hardware. This includes joint kinematics, estimated torques, Inertial Measurement Unit (IMU) readings, and power consumption statistics.

1. Communication Channels

The hardware layer publishes structured sensor data packets on the following network endpoints:

Middleware	Topic
ROS 2	`/m2_metal/hw/sensor_data`
CycloneDDS	`rt/m2_metal/hw/sensor_data`

2. SensorData Message Structure

The sensor data payload utilizes the xterra/SensorData message. This data is logically divided into centralized power diagnostics, individual actuator kinematics (arrays of size 12), and centralized IMU readings.

Raw Message Definition

xterra/msg/SensorData.msg

uint32    driver_fault
float32   driver_voltage
float32   driver_power
float32   fet_temp_c
float32[12]  q
float32[12]  dq
float32[12]  ddq
float32[12]  tau_est
float32[12]  q_current
float32[12]  d_current
float32[4]   quat
float32[3]   gyro
float32[3]   accel
float32[3]   rpy

2.1 Power & Diagnostics

These fields provide system-level health and electrical monitoring for the robot's power distribution and motor driving hardware.

Parameter	Type	Units	Description
`driver_fault`	uint32	N/A	Hardware fault code. A value of `0` indicates nominal operation.
`driver_voltage`	float32	Volts (V)	Main bus voltage supplied to the motor drivers.
`driver_power`	float32	Watts (W)	Total instantaneous power consumption of the actuator network.

2.2 Actuator Kinematics & Dynamics ([12] Arrays)

These arrays contain the mechanical and electrical state of the robot's 12 independent joints. The index 0-11 corresponds to specific leg joints (e.g., Abduction, Hip, Knee).

Parameter	Type	Units	Description
`q`	float32[12]	rad	Measured angular position of the joint.
`dq`	float32[12]	rad/s	Measured angular velocity of the joint.
`ddq`	float32[12]	rad/s²	Estimated angular acceleration of the joint.
`tau_est`	float32[12]	N·m	Estimated output torque at the joint (derived from current).
`q_current`	float32[12]	Amperes (A)	Quadrature-axis (torque-producing) current from the FOC driver.

2.3 Body IMU Data

These arrays provide the spatial orientation and acceleration of the robot's base, essential for balancing and state estimation.

Parameter	Type	Units	Description
`quat`	float32[4]	unitless	Body orientation represented as a quaternion `[x, y, z, w]`.
`gyro`	float32[3]	rad/s	Angular velocity of the base link `[roll_rate, pitch_rate, yaw_rate]`.
`accel`	float32[3]	m/s²	Linear acceleration of the base link `[x, y, z]`.
`rpy`	float32[3]	rad	Body orientation represented as Euler angles `[roll, pitch, yaw]`.

3. Implementation Examples

The following examples demonstrate how to construct subscriber nodes to ingest and parse the SensorData payload across the supported middlewares.

3.1 Sensor Subscriber (ROS 2)

Objective: Parse nested actuator arrays and IMU data using ROS 2 subscriber node.
Use Case: High-level observation, data logging (rosbags), or UI visualization (e.g., RViz).
Key Highlights:
- Accesses data via standard ROS 2 shared pointers.
- Utilizes RCLCPP_INFO_THROTTLE to maintain high-frequency underlying data ingestion while keeping the debug console readable.

#include <functional>
#include "rclcpp/rclcpp.hpp"
#include "xterra/msg/sensor_data.hpp"

class SensorDataNode : public rclcpp::Node
{
public:
    SensorDataNode() : Node("sensor_data_subscriber")
    {
        // Subscriber for sensor data with attached callback
        sub_ = this->create_subscription<xterra::msg::SensorData>(
            "/m2_metal/hw/sensor_data", 10,
            std::bind(&SensorDataNode::sensor_callback, this,
                      std::placeholders::_1));
    }

private:
    void sensor_callback(const xterra::msg::SensorData::SharedPtr msg)
    {
        // Access Front-Left Abduction joint data (Index 0)
        float joint_pos    = msg->q[0];
        float joint_vel    = msg->dq[0];
        float joint_torque = msg->tau_est[0];

        // Access IMU quaternion
        float quat_x = msg->quat[0];
        float quat_y = msg->quat[1];
        float quat_z = msg->quat[2];
        float quat_w = msg->quat[3];

        // Log data (throttled every 500ms to keep console readable)
        RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 500,
            "Joint[0] pos: %.3f, vel: %.3f, tau: %.3f | "
            "Quat: [%.2f,%.2f,%.2f,%.2f]",
            joint_pos, joint_vel, joint_torque,
            quat_x, quat_y, quat_z, quat_w);
    }

    rclcpp::Subscription<xterra::msg::SensorData>::SharedPtr sub_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SensorDataNode>());
    rclcpp::shutdown();
    return 0;
}

3.2 Sensor Subscriber (CycloneDDS)

Objective: Read real-time joint states and IMU sensor data from the hardware layer using CycloneDDS.
Use Case: High-frequency, custom controllers where minimizing read latency is critical.
Key Highlights:
- Bypasses ROS 2 abstraction layer, minimizing network latency.
- Registers a direct callback with the DDS middleware, allowing instantaneous access to data arrays as soon as network packets arrive.

#include <iostream>
#include <functional>
#include <thread>
#include <chrono>
// Include header for generated sensor data
#include "SensorData.hpp"
// Include custom header for Raw CycloneDDS subscriber abstraction
#include "dds_subscriber.hpp"

// Define the DDS Domain ID (default is 0)
const uint32_t DOMAIN_ID = 0;

// Callback function triggered upon receiving new hardware sensor data
void sensorCallback(const xterra::msg::dds_::SensorData_& msg) {
    // Power diagnostics
    std::cout << "System Voltage: " << msg.driver_voltage() << " V\n";

    // Kinematics for Joint 0 (Front-Right Abduction)
    std::cout << "Joint[0] Position: " << msg.q()[0] << " rad\n";

    // Body IMU orientation
    std::cout << "Quaternion: ["
              << msg.quat()[0] << ", " << msg.quat()[1] << ", "
              << msg.quat()[2] << ", " << msg.quat()[3] << "]\n";
}

int main() {
    auto sub = std::make_shared<DDSSubscriber<xterra::msg::dds_::SensorData_>>(
        "rt/m2_metal/hw/sensor_data",
        std::bind(&sensorCallback, std::placeholders::_1),
        DOMAIN_ID);

    std::cout << "Listening on rt/m2_metal/hw/sensor_data..." << std::endl;

    // Block thread to keep subscriber alive
    while (true) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    return 0;
}