#!/usr/bin/python3

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command, LaunchConfiguration
from launch.actions import DeclareLaunchArgument


def generate_launch_description():
    # * 这里是机器人模型路径之类的
    origincar_urdf_dir = get_package_share_directory('origincar_description')
    default_model_path = os.path.join(
        origincar_urdf_dir, "urdf", "origincar.urdf")
    default_rviz_path = os.path.join(
        origincar_urdf_dir, "rviz", "display.rviz")
    model = DeclareLaunchArgument(
        name="model", default_value=default_model_path)
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')

    # 加载机器人模型
    # 1.启动 robot_state_publisher 节点并以参数方式加载 urdf 文件
    robot_description = ParameterValue(Command(["xacro ", LaunchConfiguration("model")]),
                                       value_type=str)
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description": robot_description,
                     'use_sim_time': True, 'publish_frequency': 30.0}]
    )

    joint_state_publisher_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        parameters=[{'use_sim_time': use_sim_time}],
        arguments=[default_model_path]
    )

    # 2.启动 joint_state_publisher 节点发布非固定关节状态
    joint_state_publisher = Node(
        package="joint_state_publisher",
        executable="joint_state_publisher"
    )

    # rviz2 节点
    # *-d 为加载配置文件，这个不用看，因人而异
    rviz2 = Node(
        package="rviz2",
        executable="rviz2",
        parameters=[{'use_sim_time': use_sim_time}],
        arguments=["-d", default_rviz_path]
    )

    return LaunchDescription([
        model,
        robot_state_publisher,
        # joint_state_publisher_node,
        joint_state_publisher,
        rviz2
    ])
