#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu

class ImuCovarianceNode(Node):
    def __init__(self):
        super().__init__('imu_covariance_node')
        
        self.declare_parameter('publish_topic', '/lidar/imu_covariance').value
        self.declare_parameter('subscribe_topic', '/lidar/imu').value
        self.declare_parameter('linear_acceleration_covariance', [0.0] * 9)
        self.declare_parameter('angular_velocity_covariance', [0.0] * 9)
        self.declare_parameter('orientation_covariance', [0.0] * 9)

        self.orientation_covariance = self.get_parameter('orientation_covariance').value
        self.linear_acceleration_covariance = self.get_parameter('linear_acceleration_covariance').value
        self.angular_velocity_covariance = self.get_parameter('angular_velocity_covariance').value
        self.publish_topic = self.get_parameter('publish_topic').value
        self.subscribe_topic = self.get_parameter('subscribe_topic').value

        self.publisher = self.create_publisher(Imu, self.publish_topic, 10)


        self.subscription = self.create_subscription(
            Imu,
            self.subscribe_topic,
            self.imu_callback,
            10)

    def imu_callback(self, msg):
        # Update covariances in-place
        msg.orientation_covariance = self.orientation_covariance
        msg.linear_acceleration_covariance = self.linear_acceleration_covariance
        msg.angular_velocity_covariance = self.angular_velocity_covariance

        self.publisher.publish(msg)

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

    imu_covariance_node = ImuCovarianceNode()

    rclpy.spin(imu_covariance_node)

    imu_covariance_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
