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()   

    velodyne_raw = Node(
        package='velodyne_driver',
        executable='velodyne_driver_node',
        arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0"]
    )

    velodyne_pointcloud = Node(
        package='velodyne_pointcloud',
        executable='velodyne_transform_node',
        arguments=["--ros-args", "-p", f"calibration:={os.path.join(get_package_share_directory('sherpa'), 'config', 'vlp-16-pointcloud.yaml')}", "-p", "model:=VLP16"]
    )

    velodyne_laserscan = Node(
        package='velodyne_laserscan',
        executable='velodyne_laserscan_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_vlp16.yaml'])]
    )

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

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

    #slam_toolbox = Node(
    #    package='slam_toolbox',
    #    executable='async_slam_toolbox_node',
    #    arguments=['--ros-args', '-p', 'map_frame:=map', '-p', 'odom_frame:=camera_init', '-p', 'base_frame:=body', '-p', 'map_update_interval:=0.1', '-p', 'scan_queue_size:=256', '-p', 'position_covariance_scale:=0.1', '-p', 'use_scan_matching:=false']
    #)

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


    ld.add_action(velodyne_raw)
    ld.add_action(velodyne_pointcloud)
    ld.add_action(velodyne_laserscan)
    ld.add_action(imu)
    ld.add_action(fast_lio)
    ld.add_action(tf_laserscan)
    ld.add_action(tf_map)
    #ld.add_action(slam_toolbox)

    #ld.add_action(rviz2)

    return ld
