# import statements
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32

class IMU(Node):
    
    # Node initialization
    def __init__(self, filename):
        super().__init__("imu_api")     
        self.imu_pub = self.create_publisher(Float32, '/imu', 10)
        self.timer_ = self.create_timer(1, self.spinner)
        self.filename = filename
        self.i = 0
    
    # Creation of the function that every second publishes the value of the yaw angle reading from the default file, emulating the IMU sensor
    def spinner(self):
        msg = Float32()
        imu_list = []
        with open(self.filename, 'r') as f:
            lines = f.readlines()
            for line in lines:
                theta = float(line)
                imu_list.append(theta)
        msg.data = imu_list[self.i]
        self.imu_pub.publish(msg)
        self.i += 1

def main(args=None):
    rclpy.init(args=args)
    filename = '/home/andrea/Master_Thesis/ROS2/ros2_ws/src/my_traj/my_traj/thetas_randomized.txt'
    node = IMU(filename)
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()