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

from pathlib import Path

def generate_launch_description():

    basedir = FindPackageShare("").find("toid_lidar")

    lidar_config = PathJoinSubstitution([basedir, "params/lidar.yaml"])
    rviz_config = PathJoinSubstitution([basedir, "rviz/conf.rviz"])

    visualize = LaunchConfiguration("visualize")
    draw_markers = LaunchConfiguration("draw_markers")
    use_closest = LaunchConfiguration("use_closest")
    lidar_frame = LaunchConfiguration("lidar_frame")

    return LaunchDescription([
        DeclareLaunchArgument(
            'visualize',
            default_value='False',
            description="Whether to launch rviz2" 
        ),
        DeclareLaunchArgument(
            'draw_markers',
            default_value='False',
            description="Draw markers" 
        ),
        DeclareLaunchArgument(
            'use_closest',
            default_value='True',
            description="Use closest point for calibration" 
        ),
        DeclareLaunchArgument(
            'lidar_frame',
            default_value='lidar_frame',
            description="TF frame of the lidar" 
        ),
        Node(
            package='toid_lidar',
            executable='toid_lidar',
            output="screen",
            parameters=[
                lidar_config, 
                {
                    'closest': use_closest, 
                    'draw_markers': draw_markers
                }]
        ),
        Node(
            package='rplidar_ros',
            executable='rplidar_composition',
            output="screen",
            parameters=[lidar_config, {'frame_id': lidar_frame}]
        ),
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            output='screen',
            arguments=['-d', rviz_config],
            condition=IfCondition(visualize)
        )
    ])