import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import EmitEvent, RegisterEventHandler, DeclareLaunchArgument
from launch.events import Shutdown
from launch.event_handlers import OnProcessExit
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration

package_name = "kvelodyne"


def generate_launch_description():
    # log format
    os.environ["RCUTILS_CONSOLE_OUTPUT_FORMAT"] = (
        "[{time}] [{name}] [{severity}] {message}"
    )

    pkg_params_dir = os.path.join(get_package_share_directory("kvelodyne"), "params")

    driver_params_file = LaunchConfiguration("driver_params_file")
    default_driver_params_file = os.path.join(pkg_params_dir, "vlp16_driver_node.yaml")
    driver_params_file_launch_arg = DeclareLaunchArgument(
        "driver_params_file", default_value=default_driver_params_file
    )

    driver_node = Node(
        package="velodyne_driver",
        executable="velodyne_driver_node",
        name="vlp16_driver_node",
        output="screen",
        emulate_tty=True,
        parameters=[driver_params_file],
    )

    convert_params_file = os.path.join(pkg_params_dir, "vlp16_transform_node.yaml")
    with open(convert_params_file, "r") as f:
        convert_params = yaml.safe_load(f)["/vlp16_transform_node"]["ros__parameters"]
    convert_params["calibration"] = os.path.join(
        pkg_params_dir, convert_params["calibration"]
    )
    transform_node = Node(
        package="velodyne_pointcloud",
        executable="velodyne_transform_node",
        name="vlp16_transform_node",
        output="screen",
        emulate_tty=True,
        parameters=[convert_params],
    )

    scan_params_file = os.path.join(pkg_params_dir, "vlp16_laserscan_node.yaml")
    scan_node = Node(
        package="velodyne_laserscan",
        executable="velodyne_laserscan_node",
        name="vlp16_laserscan_node",
        output="screen",
        emulate_tty=True,
        parameters=[scan_params_file],
    )

    return LaunchDescription(
        [
            driver_params_file_launch_arg,
            driver_node,
            transform_node,
            scan_node,
            RegisterEventHandler(
                event_handler=OnProcessExit(
                    target_action=driver_node, on_exit=[EmitEvent(event=Shutdown())]
                )
            ),
        ]
    )