#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import os
from pathlib import Path

from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    package_share_dir_rm = get_package_share_directory("harvest_arm_rm")
    package_share_dir = get_package_share_directory("harvest_arm_ctrl")

    config_file = os.path.join(
        package_share_dir_rm,
        "config",
        "config.yaml"
    )

    urdf_file = os.path.join(
        package_share_dir_rm,
        "urdf",
        "urdf_rm75",
        "RM75.urdf"
    )
    meshes_dir = str(Path(urdf_file).parent)


    kine_arm_node = Node(
        package="harvest_arm_ctrl",
        executable="kine_arm",
        name="kine_arm_node",
        output="screen",
        parameters=[
            config_file,
            {
            "urdf_path": urdf_file,
            "mesh_dir": meshes_dir,
            },
        ],
    )





    return LaunchDescription([
        kine_arm_node,
    ])




'''


colcon build --packages-select harvest_arm_ctrl --symlink-install



'''