ROS 2 node

Below is a complete, self-contained Python ROS 2 Humble node using rclpy. It creates a subscriber node that listens to /mavros/state and /mavros/imu/data to log the connection state, arming status, flight mode, and quadcopter attitude in real time.

Python Node Code (inav_telemetry_monitor.py)

Python

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy

# Import standard MAVROS and ROS 2 message types
from mavros_msgs.msg import State
from sensor_msgs.msg import Imu
import math


class INAVTelemetryMonitor(Node):

    def __init__(self):
        super().__init__('inav_telemetry_monitor')

        # Best Effort / Sensor Data QoS profile for high-frequency telemetry
        sensor_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=10
        )

        # Subscriber 1: FCU Connection State & Mode
        self.state_sub = self.create_subscription(
            State,
            '/mavros/state',
            self.state_callback,
            10
        )

        # Subscriber 2: Filtered IMU Telemetry
        self.imu_sub = self.create_subscription(
            Imu,
            '/mavros/imu/data',
            self.imu_callback,
            sensor_qos
        )

        self.current_state = State()
        self.get_logger().info('INAV Telemetry Monitor Node initialized. Waiting for data...')

    def state_callback(self, msg: State):
        """Monitors system connection, arming state, and flight mode."""
        self.current_state = msg
        self.get_logger().info(
            f"[STATE UPDATE] Connected: {msg.connected} | "
            f"Armed: {msg.armed} | "
            f"Mode: {msg.mode}",
            throttle_duration_sec=2.0  # Avoid log spamming
        )

    def imu_callback(self, msg: Imu):
        """Parses raw orientation quaternions into Roll, Pitch, and Yaw degrees."""
        if not self.current_state.connected:
            return  # Skip processing if MAVROS loses link with FCU

        # Extract Quaternion orientations from ROS IMU message
        q_x = msg.orientation.x
        q_y = msg.orientation.y
        q_z = msg.orientation.z
        q_w = msg.orientation.w

        # Convert Quaternion to Euler Angles (Roll, Pitch, Yaw)
        roll_rad, pitch_rad, yaw_rad = self.euler_from_quaternion(q_x, q_y, q_z, q_w)

        # Convert Radians to Degrees
        roll_deg = math.degrees(roll_rad)
        pitch_deg = math.degrees(pitch_rad)
        yaw_deg = math.degrees(yaw_rad)

        self.get_logger().info(
            f"[IMU DATA] Roll: {roll_deg:6.1f}° | Pitch: {pitch_deg:6.1f}° | Yaw: {yaw_deg:6.1f}°",
            throttle_duration_sec=0.2  # Log output throttled to ~5 Hz
        )

    @staticmethod
    def euler_from_quaternion(x, y, z, w):
        """Convert a quaternion into euler angles (roll, pitch, yaw) in radians."""
        # Roll (x-axis rotation)
        sinr_cosp = 2 * (w * x + y * z)
        cosr_cosp = 1 - 2 * (x * x + y * y)
        roll = math.atan2(sinr_cosp, cosr_cosp)

        # Pitch (y-axis rotation)
        sinp = 2 * (w * y - z * x)
        if abs(sinp) >= 1:
            pitch = math.copysign(math.pi / 2, sinp)  # Use 90 degrees if out of range
        else:
            pitch = math.asin(sinp)

        # Yaw (z-axis rotation)
        siny_cosp = 2 * (w * z + x * y)
        cosy_cosp = 1 - 2 * (y * y + z * z)
        yaw = math.atan2(siny_cosp, cosy_cosp)

        return roll, pitch, yaw


def main(args=None):
    rclpy.init(args=args)
    node = INAVTelemetryMonitor()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info('Shutting down INAV Telemetry Monitor.')
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Step-by-Step Execution Guide

1. Save and Make Executable: Save the script to your package’s script folder or workspace (e.g., inav_telemetry_monitor.py), then make it executable:Bashchmod +x inav_telemetry_monitor.py
2. Ensure MAVROS is Running in Terminal 1:Bashros2 launch mavros node.launch fcu_url:="serial:///dev/ttyAMA0:115200"
3. Run the Python Node in Terminal 2:Bashros2 run <your_package_name> inav_telemetry_monitor.py # Or run directly via Python 3: python3 inav_telemetry_monitor.py

Key Implementation Details

• QoS Policy: The IMU subscriber uses BEST_EFFORT reliability (ReliabilityPolicy.BEST_EFFORT). Sensor streams published over MAVROS usually use best-effort delivery to minimize transport overhead.
• Quaternion Conversion: Standard ROS sensor_msgs/msg/Imu publishes orientation as a 4-element quaternion (x, y, z, w). The included euler_from_quaternion helper converts these directly into standard Roll, Pitch, and Yaw degrees.
• Log Throttling: throttle_duration_sec limits terminal output frequency without dropping subscriber thread callbacks, ensuring readable logs during rapid IMU updates.