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],
    )

    rosbridge_ws = Node(
        package='rosbridge_server',
        executable='rosbridge_websocket',
        output='screen',
        arguments=["--port", "3000"],
    )

    tf2_web_publisher = Node(
        package='tf2_web_republisher',
        executable='tf2_web_republisher_node',
        output='screen'
    )

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

