import rclpy
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from std_msgs.msg import Header
import random


class RandomJointTrajectoryNode(Node):

    def __init__(self):
        super().__init__('random_joint_trajectory_node')
        self.publisher_ = self.create_publisher(JointTrajectoryPoint, '/goal_point', 10)
        self.timer_ = self.create_timer(1.0, self.timer_callback)  # Publish every 1 second

    def timer_callback(self):
        joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']

        trajectory = JointTrajectory()
        trajectory.header = Header()
        trajectory.header.stamp = self.get_clock().now().to_msg()

        trajectory.joint_names = joint_names

        point = JointTrajectoryPoint()
        point.time_from_start.sec = 0
        point.time_from_start.nanosec = 0

        # Generate random positions for each joint
        point.positions = [random.uniform(-3.14, 3.14) for _ in range(len(joint_names))]

        trajectory.points = [point]

        self.publisher_.publish(point)


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


if __name__ == '__main__':
    main()
