import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64


class ReferencePositionGen(Node):
    def __init__(self):
        super().__init__('reference_position_gen')
        self.declare_parameter('rate', 1.0)
        self.declare_parameter('amplitude', 1.0)

        self.rate = self.get_parameter('rate').value
        self.amplitude = self.get_parameter('amplitude').value

        self.reference_pub = self.create_publisher(Float64, 'desired_position', 10)
        self.timer = self.create_timer(1.0 / self.rate, self.publish_reference)
        self.time = 0.0

    def publish_reference(self):
        msg = Float64()
        msg.data = self.amplitude * self.time  # Linearly increasing reference
        self.reference_pub.publish(msg)
        self.time += 1.0 / self.rate


def main(args=None):
    rclpy.init(args=args)
    node = ReferencePositionGen()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()