import rclpy
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
import time

class ScaledJointTrajectoryPublisher(Node):
    def __init__(self):
        super().__init__('scaled_joint_trajectory_publisher')
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            10
        )

    def send_trajectory(self, joint_positions, duration=2.0):
        """Publish a joint trajectory command to move the UR10e."""
        msg = JointTrajectory()
        msg.joint_names = [
            "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
            "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
        ]

        point = JointTrajectoryPoint()
        point.positions = joint_positions  # Target joint positions
        point.time_from_start.sec = int(duration)  # Duration in seconds

        msg.points.append(point)

        self.publisher.publish(msg)

def main():
    rclpy.init()
    node = ScaledJointTrajectoryPublisher()

    # Move to a target position
    node.send_trajectory([0.0]*6)

    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
