adding robustness

Author: jhughes50

glider/config/graph_params.yaml

@@ -1,5 +1,5 @@
-accelerometer_covariance: 0.1
-gyroscope_covariance: 0.1
+accelerometer_covariance: 0.0001
+gyroscope_covariance: 0.0001
 integration_covariance: 0.001
 bias_covariance: 0.001
 use_second_order: 0.0
@@ -16,4 +16,4 @@ odom_scale_noise: 50.0
 lag_time: 10.0
 imu_frame: "enu"
 scale_odom: true
-correct_imu: false
+correct_imu: true

glider/launch/glider-node.launch.py

@@ -53,9 +53,9 @@ def generate_launch_description():
              'use_odom': use_odom}
         ],
         remappings=[
-            ('/gps', '/ublox_gps_node/fix'),
-            ('/imu', '/zed/zed_node/imu/data'),
-            ('/mag', '/zed/zed_node/imu/mag'),
+            ('/gps', '/ublox/fix'),
+            ('/imu', '/VN100T/imu'),
+            ('/mag', '/VN100T/mag'),
             ('/odom', '/Odometry'),
         ]
     )

glider/scripts/heading.py

@@ -0,0 +1,108 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+import math
+
+
+class RpyPrinter(Node):
+    def __init__(self):
+        super().__init__('rpy_printer')
+        
+        # Create subscriber to IMU topic
+        self.imu_subscriber = self.create_subscription(
+            Imu,
+            '/VN100T/imu',  # Subscribe directly to IMU topic
+            self.imu_callback,
+            10
+        )
+        
+        # Create timer for 2Hz printing (0.5 seconds interval)
+        self.timer = self.create_timer(0.5, self.print_rpy)
+        
+        # Store latest orientation data
+        self.latest_orientation = None
+        self.latest_timestamp = None
+        
+        self.get_logger().info('Roll Pitch Yaw Printer node started')
+        self.get_logger().info('Subscribing to: /imu/data')
+        self.get_logger().info('Printing RPY at 2Hz')
+
+    def imu_callback(self, msg):
+        """
+        Callback function for IMU messages.
+        Stores the latest orientation data.
+        """
+        self.latest_orientation = msg.orientation
+        self.latest_timestamp = msg.header.stamp
+
+    def quaternion_to_euler(self, quat):
+        """
+        Convert quaternion to roll, pitch, yaw (in radians).
+        
+        Args:
+            quat: geometry_msgs.msg.Quaternion
+            
+        Returns:
+            tuple: (roll, pitch, yaw) in radians
+        """
+        x, y, z, w = quat.x, quat.y, quat.z, quat.w
+        
+        # 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 print_rpy(self):
+        """
+        Timer callback function that prints roll, pitch, yaw at 2Hz.
+        """
+        if self.latest_orientation is None:
+            self.get_logger().warn('No IMU data received yet')
+            return
+        
+        # Convert quaternion to Euler angles
+        roll, pitch, yaw = self.quaternion_to_euler(self.latest_orientation)
+        
+        # Convert to degrees for easier reading
+        roll_deg = math.degrees(roll)
+        pitch_deg = math.degrees(pitch)
+        yaw_deg = math.degrees(yaw)
+        
+        # Print with formatting
+        self.get_logger().info(
+            f'Roll: {roll_deg:8.2f}°  Pitch: {pitch_deg:8.2f}°  Yaw: {yaw_deg:8.2f}°'
+        )
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    node = RpyPrinter()
+    
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info('Shutting down Roll Pitch Yaw Printer node')
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

glider/scripts/heading_mag.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import MagneticField
+import math
+
+
+class MagnetometerHeadingNode(Node):
+    def __init__(self):
+        super().__init__('magnetometer_heading_node')
+        
+        # Create subscriber for magnetometer data
+        self.subscription = self.create_subscription(
+            MagneticField,
+            '/VN100T/mag',  # Change this to your magnetometer topic name
+            self.magnetometer_callback,
+            10
+        )
+        
+        self.get_logger().info('Magnetometer heading node started')
+        self.get_logger().info('Subscribing to topic: /imu/mag')
+
+    def magnetometer_callback(self, msg):
+        """
+        Callback function to process magnetometer data and calculate heading
+        """
+        # Extract magnetic field components
+        mag_x = msg.magnetic_field.x
+        mag_y = msg.magnetic_field.y
+        mag_z = msg.magnetic_field.z
+        
+        # Calculate heading using atan2 (yaw angle)
+        # In ENU frame: heading = atan2(mag_y, mag_x)
+        # This gives heading relative to magnetic north
+        heading_rad = math.atan2(mag_y, mag_x)
+        
+        # Convert to degrees
+        heading_deg = math.degrees(heading_rad)
+        
+        # Normalize to 0-360 degrees
+        if heading_deg < 0:
+            heading_deg += 360.0
+        
+        # Calculate magnetic field magnitude
+        magnitude = math.sqrt(mag_x**2 + mag_y**2 + mag_z**2)
+        
+        # Print the results
+        self.get_logger().info(
+            f'Magnetic Field - X: {mag_x:.3f}, Y: {mag_y:.3f}, Z: {mag_z:.3f} µT'
+        )
+        self.get_logger().info(
+            f'Heading: {heading_deg:.2f}° | Magnitude: {magnitude:.3f} µT'
+        )
+        self.get_logger().info('---')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    try:
+        magnetometer_node = MagnetometerHeadingNode()
+        rclpy.spin(magnetometer_node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        # Clean shutdown
+        if 'magnetometer_node' in locals():
+            magnetometer_node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

glider/scripts/imu_pose.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+from geometry_msgs.msg import PoseStamped
+
+
+class ImuOrientationPublisher(Node):
+    def __init__(self):
+        super().__init__('imu_orientation_publisher')
+        
+        # Create subscriber to IMU topic
+        self.imu_subscriber = self.create_subscription(
+            Imu,
+            '/vectornav/imu',  # Change this to your IMU topic name
+            self.imu_callback,
+            10
+        )
+        
+        # Create publisher for pose with orientation only
+        self.pose_publisher = self.create_publisher(
+            PoseStamped,
+            '/orientation_pose',
+            10
+        )
+        
+        self.get_logger().info('IMU Orientation Publisher node started')
+        self.get_logger().info('Subscribing to: /imu/data')
+        self.get_logger().info('Publishing to: /orientation_pose')
+
+    def imu_callback(self, msg):
+        """
+        Callback function for IMU messages.
+        Extracts orientation and publishes as PoseStamped with zero position.
+        """
+        # Create PoseStamped message
+        pose_msg = PoseStamped()
+        
+        # Set header information
+        pose_msg.header.stamp = self.get_clock().now().to_msg()
+        pose_msg.header.frame_id = msg.header.frame_id
+        
+        # Set position to (0, 0, 0)
+        pose_msg.pose.position.x = 0.0
+        pose_msg.pose.position.y = 0.0
+        pose_msg.pose.position.z = 0.0
+        
+        # Copy orientation from IMU message
+        pose_msg.pose.orientation.x = msg.orientation.x
+        pose_msg.pose.orientation.y = msg.orientation.y
+        pose_msg.pose.orientation.z = msg.orientation.z
+        pose_msg.pose.orientation.w = msg.orientation.w
+        
+        # Publish the pose
+        self.pose_publisher.publish(pose_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    node = ImuOrientationPublisher()
+    
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info('Shutting down IMU Orientation Publisher node')
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()