import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
import numpy as np

class SineWaveController(Node):
    def __init__(self):
        super().__init__('sine_controller')
        self.pub = self.create_publisher(Float64MultiArray, 'soft_arm/command', 10)
        self.timer = self.create_timer(0.05, self.timer_callback)
        self.t = 0.0

    def timer_callback(self):
        msg = Float64MultiArray()
        # 让三个段做不同相位的摆动
        # [theta1, phi1, theta2, phi2, theta3, phi3]
        theta1 = 0.5 * np.sin(self.t) + 0.5 # 弯曲程度
        phi1 = self.t # 旋转方向
        
        theta2 = 0.5 * np.sin(self.t + 1.0) + 0.5
        phi2 = self.t + 1.0
        
        theta3 = 0.5 * np.sin(self.t + 2.0) + 0.5
        phi3 = self.t + 2.0
        
        msg.data = [theta1, phi1, theta2, phi2, theta3, phi3]
        self.pub.publish(msg)
        self.t += 0.05

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