import os
import xacro
import launch
import launch_ros.actions
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():

    initial_joint_controllers_file_path = os.path.join(
        get_package_share_directory('ur_moveit_config'), 'config', 'ur_controllers.yaml'
    )

    doc = xacro.process_file(os.path.join(get_package_share_directory("ur_description"), 'urdf', 'ur.urdf.xacro'), mappings={
            "safety_limits": "true",
            "safety_pos_margin": "0.15",
            "safety_k_position": "20",
            "name": "ur",
            "ur_type": "ur5e",
            "tf_prefix": "",
            "sim_mujoco": "true",
            "simulation_controllers": str(initial_joint_controllers_file_path),
            "with_gripper": "true"
    })

    robot_desc = doc.toprettyxml(indent='  ')
    part1, part2 = robot_desc.split('?>')
    m_encoding = 'UTF-8'
    with open("current.urdf", 'w') as xfile:
        xfile.write(part1 + 'encoding=\"{}\"?>\n'.format(m_encoding) + part2)
        xfile.close()
    return launch.LaunchDescription()