import os.path

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import Command

def generate_launch_description():

    ld = LaunchDescription()  

    pkg_name = 'sherpa'
    pkg_share = get_package_share_directory(pkg_name)
    pkg_prefix = os.path.dirname(pkg_share) 

    world_path = os.path.join(pkg_share, 'worlds', 'asd_course.world')
    xacro_file = os.path.join(pkg_share, 'urdf', 'sherpa.xacro')

    # create nodes

    gz_env = SetEnvironmentVariable(
        name='GZ_SIM_RESOURCE_PATH',
        value=[pkg_prefix]
    )

    gz = ExecuteProcess(
        cmd=[
            'ros2', 'launch', 'ros_gz_sim', 'gz_sim.launch.py',
            f'gz_args:=-r {world_path}'
        ],
        output='screen'
    )

    state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{
            'robot_description': Command(['xacro ', xacro_file])
        }]
    )

    controller = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['ackermann_controller'],
        #output='screen'
    )

    joint_state_broadcaster = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
        parameters=[{'use_sim_time': True}],
        #output='screen'
    )

    static_tf = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='static_tf_base_to_lidar',
        arguments=[
            '0', '0', '0',
            '0', '0', '0',
            'velodyne',
            'lidar_link',
        ]          
    )

    robot = ExecuteProcess(
        cmd=[
            'ros2', 'run', 'ros_gz_sim', 'create',
            '-name', 'sherpa',
            '-topic', 'robot_description',
            '-x', '0', '-y', '0', '-z', '2'
        ],
        output='screen'
    )

    twist_stamper = Node(
        package="twist_stamper",
        executable="twist_stamper",
        remappings=[
            ('/cmd_vel_out', '/ackermann_controller/reference'),
        ],
        output='screen'
    )

    gz_bridge = Node(
        package="ros_gz_bridge",
        executable="parameter_bridge",
        arguments=[
            "--ros-args",
            "-p",
            f"config_file:={os.path.join(pkg_share, 'config', 'gz_bridge.yaml')}",
        ]
    )


    rviz2 = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', f"{os.path.join(get_package_share_directory('sherpa'), 'config', 'sherpa.rviz')}"]
    )

    ld.add_action(gz_env)
    ld.add_action(gz)
    ld.add_action(state_publisher)
    ld.add_action(static_tf)
    ld.add_action(robot)
    ld.add_action(gz_bridge)
    ld.add_action(joint_state_broadcaster)
    ld.add_action(controller)
    ld.add_action(twist_stamper)
    ld.add_action(rviz2)

    return ld