def export_robot_to_xacro_files():
    """
    Exports the entire 'robot' in several xacro files.
    One xacro file per kinematic chain (<ROBOT>_legs.xacro, <ROBOT>_arms.xacro, <ROBOT>_torso.xacro...)
    Xacro file for specific parts of the robot (<ROBOT>_fingers.xacro, <ROBOT>_sensors.xacro)
    One xacro file for visual elements (<ROBOT>_visual_collision.xacro, <ROBOT>_material.xacro)
    One xacro file per type of element needed for gazebo simulation (<ROBOT>_Gazebo.xacro, <ROBOT>_Transmission.xacro)
    One generic robot file which includes all the other ones (<ROBOT>_robot.xacro)
    """
    global robot, OUTPUT, NAME
    doc = Document()
    root = doc.createElement("robot")
    doc.appendChild(root)
    root.setAttribute("xmlns:xacro","http://www.ros.org/wiki/xacro")
    root.setAttribute("name",robot.name)
    root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_visual_collisions.xacro'))
    create_visual_xacro()
    for i in XACRO_DICO.keys():
        print("exporting " + NAME + '_' + i + '.xacro')
        if i.find("eye") != -1:
            export_kinematic_chain_to_xacro(i,'HeadRoll_link','HeadRoll_link')
        else:
            export_kinematic_chain_to_xacro(i)
        filenamerobot = NAME + '_' + i + '.xacro'
        root.appendChild(ur.short(doc,'xacro:include','filename',filenamerobot))
## Transmission elements not available from Aldebaran libraries yet
#    export_robot_element('Transmission')
#    root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_Transmission.xacro'))
## Gazebo Plugin not available from Aldebaran libraries yet
#    export_robot_element('Gazebo')
#    root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_Gazebo.xacro'))
    root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_sensors.xacro'))
    export_list_to_xacro(['_frame'],OUTPUT[0:OUTPUT.find('.')] + '_sensors.xacro')
    root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_fingers.xacro'))
    export_list_to_xacro(['Finger','Thumb'],OUTPUT[0:OUTPUT.find('.')] + '_fingers.xacro')
    if NAME == 'pepper':
        root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_wheels.xacro'))
        export_list_to_xacro(['Wheel'],OUTPUT[0:OUTPUT.find('.')] + '_wheels.xacro')

    filename = OUTPUT[0:OUTPUT.find('.')]+ '_robot.xacro'
    write_comments_in_xacro(doc,filename)
    print('output directory : ' + OUTPUT[0:OUTPUT.rfind('/')+1])
def export_robot_to_xacro_files():
    """
    Exports the entire 'robot' in several xacro files.
    One xacro file per kinematic chain (<ROBOT>_legs.xacro, <ROBOT>_arms.xacro, <ROBOT>_torso.xacro...)
    Xacro file for specific parts of the robot (<ROBOT>_fingers.xacro, <ROBOT>_sensors.xacro)
    One xacro file for visual elements (<ROBOT>_visual_collision.xacro, <ROBOT>_material.xacro)
    One xacro file per type of element needed for gazebo simulation (<ROBOT>_Gazebo.xacro, <ROBOT>_Transmission.xacro)
    One generic robot file which includes all the other ones (<ROBOT>_robot.xacro)
    """
    global robot, OUTPUT, NAME
    doc = Document()
    root = doc.createElement("robot")
    doc.appendChild(root)
    root.setAttribute("xmlns:xacro","http://www.ros.org/wiki/xacro")
    root.setAttribute("name",robot.name)
    root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_visual_collisions.xacro'))
    create_visual_xacro()
    for i in XACRO_DICO.keys():
        print("exporting " + NAME + '_' + i + '.xacro')
        if i.find("eye") != -1:
            export_kinematic_chain_to_xacro(i,'HeadRoll_link','HeadRoll_link')
        else:
            export_kinematic_chain_to_xacro(i)
        filenamerobot = NAME + '_' + i + '.xacro'
        root.appendChild(ur.short(doc,'xacro:include','filename',filenamerobot))
## Transmission elements not available from Aldebaran libraries yet
#    export_robot_element('Transmission')
#    root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_Transmission.xacro'))
## Gazebo Plugin not available from Aldebaran libraries yet
#    export_robot_element('Gazebo')
#    root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_Gazebo.xacro'))
    root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_sensors.xacro'))
    export_list_to_xacro(['_frame'],OUTPUT[0:OUTPUT.find('.')] + '_sensors.xacro')
    root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_fingers.xacro'))
    export_list_to_xacro(['Finger','Thumb'],OUTPUT[0:OUTPUT.find('.')] + '_fingers.xacro')
    if NAME == 'pepper':
        root.appendChild(ur.short(doc,'xacro:include','filename', NAME + '_wheels.xacro'))
        export_list_to_xacro(['Wheel'],OUTPUT[0:OUTPUT.find('.')] + '_wheels.xacro')

    filename = OUTPUT[0:OUTPUT.find('.')]+ '_robot.xacro'
    write_comments_in_xacro(doc,filename)
    print('output directory : ' + OUTPUT[0:OUTPUT.rfind('/')+1])
def create_visual_xacro():
    global robot
    """
    Creates a <ROBOT>_visual_collision.xacro file with xacro macros for visual and collision tags of the urdf.
    This function creates xacro macros for visualisation and collisions. It checks if the meshes are on the computer(nao only) and set the 'meshes_installed' property accordingly
    """
    global OUTPUT
    global MESH_VERSION
    global NAME
    global LINKS_DICO
    global VISU_DICO
    global OFFSETS_DICO
    global MESHPKG
    prefix = "insert_visu_"
    doc = Document()
    root = doc.createElement("robot")
    doc.appendChild(root)
    root.setAttribute("xmlns:xacro","http://www.ros.org/wiki/xacro")

    cmd = "rospack find "+ NAME + MESHPKG
    try:
        path_mesh_pkg = subprocess.check_output(cmd, stderr=subprocess.STDOUT, shell=True)[:-1]
    except:
        print("unable to find "+ NAME + MESHPKG + " package")
        sys.exit(0)
    # Set Mesh path
    if NAME =='nao':
        node = ur.short(doc,'xacro:property','name','PI_2')
        node.setAttribute('value',str(math.pi/2.0))
        root.appendChild(node)
        node = ur.short(doc,'xacro:property','name','meshes_installed')

        if os.path.isdir(os.path.join(path_mesh_pkg,'meshes',MESH_VERSION)):
	    node.setAttribute('value',"true")
        else:
	    node.setAttribute('value',"false")
        root.appendChild(node)

    #Insert xacro macro
    for link in robot.links:
        (tempVisu,tempCol) = adjustMeshPath(path_mesh_pkg,link)
        if robot.links[link].visual != None:
            robot.links[link].xacro = 'xacro:' + prefix + robot.links[link].name
            node = ur.short(doc,'xacro:macro','name', prefix + robot.links[link].name)
            if NAME=='nao':
            # add xacro condition macro to handle the absence of meshes on the system
	        node2 = ur.short(doc,'xacro:unless','value',"${meshes_installed}")
                if tempVisu != None:
                    node2.appendChild(tempVisu.to_xml(doc))
                if tempCol != None:
                    node2.appendChild(tempCol.to_xml(doc))
	        node.appendChild(node2)
	        node3 = ur.short(doc,'xacro:if','value',"${meshes_installed}")
	        node3.appendChild(robot.links[link].visual.to_xml(doc))
	        node3.appendChild(robot.links[link].collision.to_xml(doc))
	        node.appendChild(node3)
            else:
                node.appendChild(robot.links[link].visual.to_xml(doc))
                node.appendChild(robot.links[link].collision.to_xml(doc))
	    root.appendChild(node)
            robot.links[link].visual = None
	    robot.links[link].collision = None
    filename = OUTPUT[0:OUTPUT.rfind('.')] + '_visual_collisions.xacro'
    write_comments_in_xacro(doc, filename)
def create_visual_xacro():
    """Create a <ROBOT>_visual_collision.xacro file.

    with xacro macros for visual and collision tags of the urdf.
    This function creates xacro macros for visualisation and collisions.
    It checks if the meshes are on the computer and set the 'meshes_installed'
    property accordingly
    """
    global robot
    global OUTPUT
    global MESH_VERSION
    global NAME
    global LINKS_DICO
    global VISU_DICO
    global OFFSETS_DICO
    global MESHPKG
    prefix = 'insert_visu_'
    doc = Document()
    root = doc.createElement('robot')
    doc.appendChild(root)
    root.setAttribute("xmlns:xacro", "http://www.ros.org/wiki/xacro")

    cmd = 'rospack find ' + NAME + MESHPKG
    try:
        path_mesh_pkg = subprocess.check_output(cmd, stderr=subprocess.STDOUT,
            shell=True)[:-1]
    except:
        print('unable to find ' + NAME + MESHPKG + ' package')
        sys.exit(0)
    # Set Mesh path
    if NAME == 'nao':
        node = ur.short(doc, 'xacro:property', 'name', 'PI_2')
        node.setAttribute('value', str(math.pi/2.0))
        root.appendChild(node)
        node = ur.short(doc, 'xacro:property', 'name', 'meshes_installed')

        if os.path.isdir(os.path.join(path_mesh_pkg, 'meshes', MESH_VERSION)):
            node.setAttribute('value', 'true')
        else:
            node.setAttribute('value', 'false')
        root.appendChild(node)

    # Insert xacro macro
    for link in robot.links:
        (tempVisu, tempCol) = adjustMeshPath(path_mesh_pkg, link)
        if robot.links[link].visual is not None:
            robot.links[link].xacro = 'xacro:' + prefix + \
                robot.links[link].name
            node = ur.short(doc, 'xacro:macro', 'name', prefix +
                robot.links[link].name)
            if NAME == 'nao':
                # add xacro condition macro to handle the absence of meshes
                node2 = ur.short(doc, 'xacro:unless', 'value',
                    '${meshes_installed}')
                if tempVisu is not None:
                    node2.appendChild(tempVisu.to_xml(doc))
                if tempCol is not None:
                    node2.appendChild(tempCol.to_xml(doc))
                node.appendChild(node2)
                node3 = ur.short(doc, 'xacro:if', 'value', '${meshes_installed}')
                node3.appendChild(robot.links[link].visual.to_xml(doc))
                node3.appendChild(robot.links[link].collision.to_xml(doc))
                node.appendChild(node3)
            else:
                node.appendChild(robot.links[link].visual.to_xml(doc))
                node.appendChild(robot.links[link].collision.to_xml(doc))
            root.appendChild(node)
        robot.links[link].visual = None
        robot.links[link].collision = None
    filename = OUTPUT[0:OUTPUT.rfind('.')] + '_visual_collisions.xacro'
    write_comments_in_xacro(doc, filename)