#!/usr/bin/env python3

import rospy
from industrial_msgs.msg import RobotStatus, RobotMode
from fanuc_msg_translator_msgs.msg import FanucRobotStatus

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

def robot_status_callback(msg):
    """
    Callback function for the /robot_status topic. This function is called every time a new message is published on
    the topic. The function converts the message from the industrial_msgs/RobotStatus message type to the custom
    fanuc_msg_translator_msgs/FanucRobotStatus message type and publishes the converted message on the
    /msg_translator/robot_status topic.

    :param msg: The message published on the /robot_status topic
    :type msg: industrial_msgs/RobotStatus
    """
    # Create a publisher for the result
    result_publisher = rospy.Publisher(ns + 'bridge/robot_status', FanucRobotStatus, queue_size=10)

    # Create a new FanucRobotStatus message
    robot_status = FanucRobotStatus()

    # Set the values of the new message
    robot_status.header.stamp = msg.header.stamp
    if msg.mode.val == RobotMode.MANUAL:
        robot_status.tp_enabled.val = 1
    elif msg.mode.val == RobotMode.AUTO:
        robot_status.tp_enabled.val = 0
    elif msg.mode.val == RobotMode.UNKNOWN:
        robot_status.tp_enabled.val = -1
    robot_status.e_stopped = msg.e_stopped
    robot_status.drives_powered = msg.drives_powered
    robot_status.motion_possible = msg.motion_possible
    robot_status.in_motion = msg.in_motion
    robot_status.in_error = msg.in_error
    robot_status.error_code = msg.error_code

    # Publish the new message
    result_publisher.publish(robot_status)
def main():
    # Initialize the node
    rospy.init_node('fanuc_robot_status_converter_node')

    # Subscribe to the /robot_status topic
    rospy.Subscriber(ns + 'robot_status', RobotStatus, robot_status_callback)

    rospy.loginfo("[Robot Status Converter] Node initialized!")

    # Spin the node
    rospy.spin()


if __name__ == '__main__':
    main()
