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

import os

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 = get_package_share_directory("harvest_arm_rm")

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

    urdf_file = os.path.join(
        package_share_dir,
        "urdf",
        "urdf_rm75",
        "RM75.urdf"
    )

    with open(urdf_file, "r") as f:
        robot_description = f.read()

    realman_arm_node = Node(
        package="harvest_arm_rm",
        executable="realman_arm",
        name="realman_arm_node",
        output="screen",
        parameters=[config_file],
    )

    realman_rviz_node = Node(
        package="harvest_arm_rm",
        executable="realman_rviz",
        name="realman_rviz_node",
        output="screen",
        parameters=[config_file],
    )

    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="screen",
        parameters=[
            {
                "robot_description": robot_description,
            }
        ],
    )

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
    )

    return LaunchDescription([
        robot_state_publisher_node,
        realman_arm_node,
        realman_rviz_node,
        rviz_node,
    ])