ROS2 Python script to publish custom joint states

The following script will generate sinusodial joint states and publish them to a ROS2 topic using Python.

The names and joints variables vary by robot, the example below is for a 7-DoF robot + one End-Effector DOF (Franka research 3).

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
import math
from time import time

class SinusoidalJointPublisher(Node):
    def __init__(self):
        super().__init__('sinusoidal_joint_publisher')
        self.publisher = self.create_publisher(JointState, 'joint_states', 10)
        self.timer = self.create_timer(0.1, self.timer_callback)  # 10 Hz
        self.start_time = time()

    def timer_callback(self):
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.name = ['joint1']
        
        # Calculate sinusoidal position (amplitude = 1, frequency = 0.5 Hz)
        t = time() - self.start_time
        position = math.sin(2 * math.pi * 0.5 * t)
        
        msg.name = [
            "fr3_joint1",
            "fr3_joint2",
            "fr3_joint3",
            "fr3_joint4",
            "fr3_joint5",
            "fr3_joint6",
            "fr3_joint7",
            "fr3_finger_joint1",
            "fr3_finger_joint2",
        ]
        msg.position = [
            0.0,
            0.5,
            0.5,
            0.5,
            position,
            0.5,
            0.5,
            -0.05,
            -0.05,
        ]
        msg.velocity = [0.0]
        msg.effort = [0.0]
        
        self.publisher.publish(msg)
        self.get_logger().info(f'Publishing joint position: {position}')

def main(args=None):
    rclpy.init(args=args)
    node = SinusoidalJointPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()