from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import os

def generate_launch_description():

    pkg_share = FindPackageShare("").find('toid_bt')
    bt_params = os.path.join(pkg_share, 'params', 'bt_params.yaml')

    visualize = LaunchConfiguration("visualize")
    visualize_arg = DeclareLaunchArgument(
        'visualize',
        default_value='False',
        description="Whether to launch rviz2" 
    )

    use_mock = LaunchConfiguration("use_mock")
    use_mock_arg = DeclareLaunchArgument(
        'use_mock',
        default_value='True',
        description="Whether to use mock controller" 
    )

    bt_executor = Node(
        package='toid_bt',
        executable='toid_bt',
        output='screen',
        emulate_tty=True,
        parameters=[bt_params],
    )

    return LaunchDescription([
        visualize_arg,
        use_mock_arg,
        bt_executor,
    ])