import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TwistStamped
from control_msgs.msg import JointJog
from std_msgs.msg import Float64MultiArray

TWIST_TOPIC = "/left_servo_node/delta_twist_cmds"
JOINT_TOPIC = "/left_servo_node/delta_joint_cmds"
EEF_FRAME_ID = "openarm_left_link7"
BASE_FRAME_ID = "openarm_body_link0"

class ServoControlNode(Node):
    def __init__(self):
        super().__init__('servo_control_node')
        self.get_logger().info('正在初始化Servo控制节点...')
        # self.jointJogPublisher_ = self.create_publisher(JointJog, JOINT_TOPIC, 10)
        # self.twistStampedPublisher_ = self.create_publisher(TwistStamped, TWIST_TOPIC, 10)
        self.float64MultiArrayPublisher_ = self.create_publisher(Float64MultiArray, "/left_forward_position_controller/commands", 10)
        self.timer = self.create_timer(0.05, self.timer_callback)  # 每20ms发布一次
        self.count = 0.0
        self.countFlag = False
 
    def timer_callback(self):
        msg = Float64MultiArray()
        if self.count > 1.5:
            self.countFlag = True
        if self.count < -1.0:
            self.countFlag = False
        if self.countFlag:
            self.count -= 0.05
        else:
            self.count += 0.05
        msg.data = [self.count, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  # 示例速度

        # msg = JointJog()
        # msg.header.stamp = self.get_clock().now().to_msg()
        # msg.joint_names = ["openarm_left_joint1"]
        # msg.velocities = [10.0]  # 示例速度
        # msg.displacements = [1.0]  # 示例位移
        # # 设置持续时间为1秒
        # try:
        #     msg.duration.sec = 1
        # except Exception:
        #     # 如果字段不存在或不能直接赋值，忽略以保持兼容性
        #     pass
        # msg.header.frame_id = BASE_FRAME_ID
        # self.jointJogPublisher_.publish(msg)
        self.float64MultiArrayPublisher_.publish(msg)
        self.get_logger().info('Published servo command')

def main():
    rclpy.init()
    node = ServoControlNode()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()