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
from launch.actions import ExecuteProcess


def generate_launch_description():
    ld = LaunchDescription()
    robot_name_in_model = 'mycar'
    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')
    # * ###################gazebo
    gazebo_world_path = os.path.join(origincar_urdf_dir, 'world/test.world')
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
             '-s', 'libgazebo_ros_factory.so', gazebo_world_path],
        output='screen')

    spawn_entity_cmd = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', robot_name_in_model,
                   '-file', default_model_path],
        output='screen'
    )

    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}]
    )

    # 2.启动 joint_state_publisher 节点发布非固定关节状态
    joint_state_publisher = Node(
        package="joint_state_publisher",
        executable="joint_state_publisher"
    )
    # *###################rviz
    # rviz2 节点
    # *-d 为加载配置文件，这个不用看，因人而异
    rviz2 = Node(
        package="rviz2",
        executable="rviz2",
        arguments=["-d", default_rviz_path],
        parameters=[{'use_sim_time': use_sim_time}]
    )
    ld.add_action(model)
    ld.add_action(start_gazebo_cmd)
    ld.add_action(spawn_entity_cmd)
    ld.add_action(robot_state_publisher)
    # ld.add_action(joint_state_publisher)
    # ld.add_action(rviz2)
    return ld
