#!/usr/bin/python

import roslib
import rospy
import xml.dom.minidom

def get_param(name, value=None):
    private = "~%s" % name
    if rospy.has_param(private):
        return rospy.get_param(private)
    elif rospy.has_param(name):
        return rospy.get_param(name)
    else:
        return value

def set_param(name, value):
    private = "~%s" % name
    rospy.set_param(private, value)


def set_param():
    description = get_param('robot_description_semantic')
    group_description = xml.dom.minidom.parseString(description).getElementsByTagName('group')
    effector_description = xml.dom.minidom.parseString(description).getElementsByTagName('end_effector')
    for group_name in group_description:
        links = []
        name = group_name.getAttribute('name')
        for child in group_name.childNodes:
            if child.nodeType is child.TEXT_NODE:
                continue
            links.append(child.getAttribute('name'))
        rospy.set_param('~' + name, links)
    for effector_name in effector_description:
        if effector_name.nodeType is effector_name.TEXT_NODE:
            continue
        link_name = effector_name.getAttribute('parent_link')
        group_name = effector_name.getAttribute('parent_group')
        rospy.set_param('/end_effector_link/' + group_name, link_name)



if __name__ == '__main__':
    try:
        rospy.init_node('link_group_publisher')
        set_param()
    except rospy.ROSInterruptException: pass
