import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node

package_name = "gr_vision60"


def generate_launch_description():

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

    vlp16_launch_dir = os.path.join(get_package_share_directory("kvelodyne"), "launch")
    vlp16_params_file = os.path.join(
        get_package_share_directory("gr_vision60"), "params", "vlp16_driver_node.yaml"
    )

    launch_vlp16 = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(vlp16_launch_dir, "vlp16.launch.py")
        ),
        launch_arguments={"driver_params_file": vlp16_params_file}.items(),
    )

    imu_params_file = os.path.join(
        get_package_share_directory("gr_vision60"), "params", "imu_gx5.yaml"
    )
    gx5_launch_dir = os.path.join(
        get_package_share_directory("microstrain_inertial_driver"), "launch"
    )
    launch_gx5 = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(gx5_launch_dir, "microstrain_launch.py")
        ),
        launch_arguments={"params_file": imu_params_file}.items(),
    )

    imu_process_node = Node(
        package="gr_vision60",
        executable="imu_9axis_processor",
        name="imu_9axis_processor",
        output="screen",
    )

    return LaunchDescription(
        [
            launch_vlp16,
            launch_gx5,
            imu_process_node,
        ]
    )