import time
import random
from arcros import create_publisher


def main():
    with create_publisher('sensor_data', description='Simulated sensor data from encoders and IMU') as pub:
        rate_hz = 10
        print(f"Publishing at {rate_hz} Hz")
        print("Press Ctrl+C to stop\n")
        
        try:
            while True:
                data = {
                    'e1': random.uniform(40.0, 50.0),
                    'e2': random.uniform(-15.0, -10.0),
                    'imu': random.uniform(175.0, 185.0),
                    'timestamp': time.time()
                }
                
                pub.publish(data)
                print(f"Published: e1={data['e1']:.2f}, e2={data['e2']:.2f}, imu={data['imu']:.2f}")
                
                time.sleep(1.0 / rate_hz)
                
        except KeyboardInterrupt:
            print("\nStopped publishing.")


if __name__ == "__main__":
    main()
