import os.path

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.conditions import IfCondition

def generate_launch_description():

    ld = LaunchDescription()   

    livox_driver = Node(
        package='livox_ros2_driver',
        executable='livox_ros2_driver_node',
        arguments=[]
    )

    imu = Node(
        package = 'witmotion_ros',
        executable = 'witmotion_ros_node',
        parameters = [os.path.join(get_package_share_directory('sherpa'), 'config', 'wt931.yml')]
    )

    fast_lio = Node(
        package='fast_lio',
        executable='fastlio_mapping',
        output='screen',
        parameters=[PathJoinSubstitution([os.path.join(get_package_share_directory('sherpa'), 'config'), 'fastlio2_mid40.yaml'])]
    )

    tf_laserscan = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['0.0', '0', '0.0', '0', '0', '0', '1', 'body', 'frame_default']
    )

    tf_map = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['0.0', '0', '0.0', '0', '0', '0', '1', 'camera_init', 'map']
    )


    ld.add_action(livox_driver)
    ld.add_action(imu)
    ld.add_action(fast_lio)
    ld.add_action(tf_laserscan)
    ld.add_action(tf_map)

    return ld
