#!/usr/bin/env python

import rospy
from std_msgs.msg import String
from sensor_msgs.msg import JointState
from geometry_msgs.msg import PoseStamped

import numpy as np
import xml.etree.ElementTree as ET

from kuka_eki_interface import UDPClient, xyzabc_in_mm_deg_to_pose


def state_from_xml(xml):
    root = ET.fromstring(xml)
    axes = root.find('Axis').attrib
    joint_state = JointState()
    now = rospy.Time.now()
    joint_state.header.stamp = now
    joint_state.name = [
        'joint_a1',
        'joint_a2',
        'joint_a3',
        'joint_a4',
        'joint_a5',
        'joint_a6',
    ]
    joint_state.position = np.deg2rad([
        float(axes['A1']),
        float(axes['A2']),
        float(axes['A3']),
        float(axes['A4']),
        float(axes['A5']),
        float(axes['A6']),
    ])

    cart = root.find('Pos').attrib
    pose_stamped = PoseStamped()
    pose_stamped.header.frame_id = 'base_link'
    pose_stamped.header.stamp = now
    pose_stamped.pose = xyzabc_in_mm_deg_to_pose([
        float(cart['X']),
        float(cart['Y']),
        float(cart['Z']),
        float(cart['A']),
        float(cart['B']),
        float(cart['C'])])
    return joint_state, pose_stamped


if __name__ == "__main__":
    joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10)
    robot_state_pub = rospy.Publisher('robot_state', PoseStamped, queue_size=10)
    robot_ip = rospy.get_param('/eki/robot_ip')
    robot_state_port = rospy.get_param('/eki/robot_state_port')
    conn = UDPClient((robot_ip, int(robot_state_port)))
    conn.settimeout(0.1)
    conn.send(b'0')
    name = 'kuka_eki_state_publisher'
    rospy.init_node(name)
    while not rospy.is_shutdown():
        try:
            data = conn.recv(1024)
            joint_state, robot_state = state_from_xml(data)
            joint_state_pub.publish(joint_state)
            robot_state_pub.publish(robot_state)
        except:
            pass
