import os
import pathlib
import launch
from launch_ros.actions import Node
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from webots_ros2_driver.webots_launcher import WebotsLauncher
from webots_ros2_driver.utils import controller_url_prefix

def generate_launch_description():
    package_dir = get_package_share_directory('sweet_webots_robot')
    controllers_dir = os.path.join( package_dir,'controllers')
    robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'sweet_robot.urdf')).read_text()

    webots = WebotsLauncher(
        world=os.path.join(package_dir, 'worlds', 'sweet_robot.wbt')
        #world=os.path.join(package_dir, 'worlds', 'my_world.wbt')
    )

    my_robot_driver = Node(
        package='webots_ros2_driver',
        executable='driver',
        output='screen',
        additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'tracked_robot'},
        #additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'my_robot'},
        parameters=[
            {'robot_description': robot_description},
        ]
    )
    pkg_path = get_package_share_directory(
        'sweet_webots_robot'
    )

    urdf_file = os.path.join(
        pkg_path,
        'urdf',
        'rviz_robot.urdf'
    )

    with open(urdf_file, 'r') as f:
        robot_description = f.read()
    robot_state_publisher = Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    name='robot_state_publisher',
    parameters=[
        {
            'robot_description': robot_description
        }
    ],
    output='screen'
)
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen'
    )

    path_visualizer = Node(
        package='sweet_webots_robot',
        executable='path_visualizer',
        name='path_visualizer',
        output='screen'
    )
    return LaunchDescription([
        webots,
        my_robot_driver,
        robot_state_publisher,
        rviz_node,
        #path_visualizer,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=webots,
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
            )
        )
    ])