|
| 1 | +#!/usr/bin/env python3 |
| 2 | + |
| 3 | +import rclpy |
| 4 | +from rclpy.node import Node |
| 5 | +from sensor_msgs.msg import Imu |
| 6 | +import math |
| 7 | + |
| 8 | + |
| 9 | +class RpyPrinter(Node): |
| 10 | + def __init__(self): |
| 11 | + super().__init__('rpy_printer') |
| 12 | + |
| 13 | + # Create subscriber to IMU topic |
| 14 | + self.imu_subscriber = self.create_subscription( |
| 15 | + Imu, |
| 16 | + '/VN100T/imu', # Subscribe directly to IMU topic |
| 17 | + self.imu_callback, |
| 18 | + 10 |
| 19 | + ) |
| 20 | + |
| 21 | + # Create timer for 2Hz printing (0.5 seconds interval) |
| 22 | + self.timer = self.create_timer(0.5, self.print_rpy) |
| 23 | + |
| 24 | + # Store latest orientation data |
| 25 | + self.latest_orientation = None |
| 26 | + self.latest_timestamp = None |
| 27 | + |
| 28 | + self.get_logger().info('Roll Pitch Yaw Printer node started') |
| 29 | + self.get_logger().info('Subscribing to: /imu/data') |
| 30 | + self.get_logger().info('Printing RPY at 2Hz') |
| 31 | + |
| 32 | + def imu_callback(self, msg): |
| 33 | + """ |
| 34 | + Callback function for IMU messages. |
| 35 | + Stores the latest orientation data. |
| 36 | + """ |
| 37 | + self.latest_orientation = msg.orientation |
| 38 | + self.latest_timestamp = msg.header.stamp |
| 39 | + |
| 40 | + def quaternion_to_euler(self, quat): |
| 41 | + """ |
| 42 | + Convert quaternion to roll, pitch, yaw (in radians). |
| 43 | + |
| 44 | + Args: |
| 45 | + quat: geometry_msgs.msg.Quaternion |
| 46 | + |
| 47 | + Returns: |
| 48 | + tuple: (roll, pitch, yaw) in radians |
| 49 | + """ |
| 50 | + x, y, z, w = quat.x, quat.y, quat.z, quat.w |
| 51 | + |
| 52 | + # Roll (x-axis rotation) |
| 53 | + sinr_cosp = 2 * (w * x + y * z) |
| 54 | + cosr_cosp = 1 - 2 * (x * x + y * y) |
| 55 | + roll = math.atan2(sinr_cosp, cosr_cosp) |
| 56 | + |
| 57 | + # Pitch (y-axis rotation) |
| 58 | + sinp = 2 * (w * y - z * x) |
| 59 | + if abs(sinp) >= 1: |
| 60 | + pitch = math.copysign(math.pi / 2, sinp) # Use 90 degrees if out of range |
| 61 | + else: |
| 62 | + pitch = math.asin(sinp) |
| 63 | + |
| 64 | + # Yaw (z-axis rotation) |
| 65 | + siny_cosp = 2 * (w * z + x * y) |
| 66 | + cosy_cosp = 1 - 2 * (y * y + z * z) |
| 67 | + yaw = math.atan2(siny_cosp, cosy_cosp) |
| 68 | + |
| 69 | + return roll, pitch, yaw |
| 70 | + |
| 71 | + def print_rpy(self): |
| 72 | + """ |
| 73 | + Timer callback function that prints roll, pitch, yaw at 2Hz. |
| 74 | + """ |
| 75 | + if self.latest_orientation is None: |
| 76 | + self.get_logger().warn('No IMU data received yet') |
| 77 | + return |
| 78 | + |
| 79 | + # Convert quaternion to Euler angles |
| 80 | + roll, pitch, yaw = self.quaternion_to_euler(self.latest_orientation) |
| 81 | + |
| 82 | + # Convert to degrees for easier reading |
| 83 | + roll_deg = math.degrees(roll) |
| 84 | + pitch_deg = math.degrees(pitch) |
| 85 | + yaw_deg = math.degrees(yaw) |
| 86 | + |
| 87 | + # Print with formatting |
| 88 | + self.get_logger().info( |
| 89 | + f'Roll: {roll_deg:8.2f}° Pitch: {pitch_deg:8.2f}° Yaw: {yaw_deg:8.2f}°' |
| 90 | + ) |
| 91 | + |
| 92 | + |
| 93 | +def main(args=None): |
| 94 | + rclpy.init(args=args) |
| 95 | + |
| 96 | + node = RpyPrinter() |
| 97 | + |
| 98 | + try: |
| 99 | + rclpy.spin(node) |
| 100 | + except KeyboardInterrupt: |
| 101 | + node.get_logger().info('Shutting down Roll Pitch Yaw Printer node') |
| 102 | + finally: |
| 103 | + node.destroy_node() |
| 104 | + rclpy.shutdown() |
| 105 | + |
| 106 | + |
| 107 | +if __name__ == '__main__': |
| 108 | + main() |
0 commit comments