#!/usr/bin/env python3

import rospy
from sensor_msgs.msg import JointState

# Namespace of the node
ns = rospy.get_namespace()

joint_states_publisher = rospy.Publisher(ns + 'bridge/joint_states', JointState, queue_size=10)


def joint_states_callback(msg):
    """
    Callback function for the /joint_states topic. Publishes the received message to the /bridge/joint_states topic.

    :param msg: The received message from the /joint_states topic
    :type msg: JointState
    """
    global joint_states_publisher

    joint_states_publisher.publish(msg)


def main():
    # Initialize the node
    rospy.init_node('fanuc_joint_states_converter_node')

    # Subscribe to the /robot_status topic
    rospy.Subscriber(ns + 'joint_states', JointState, joint_states_callback)

    rospy.loginfo("[Joint States Converter] Node initialized!")

    # Spin the node
    rospy.spin()


if __name__ == '__main__':
    main()
