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

def generate_launch_description():
    pkg_share = FindPackageShare("").find('toid_navigation')
    params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
    map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'ros_control.rviz')
    

    description_pkg_share = FindPackageShare("").find('toid_bot_description')
    default_model_path = os.path.join(
        description_pkg_share, 
        'src', 
        'toid_bot_description.urdf'
    )

    starting_position = LaunchConfiguration("start_position")
    visualize = LaunchConfiguration("visualize")

    starting_position_arg = DeclareLaunchArgument(
        'start_position',
        default_value='0 0 0 0 0 0',
        description="Set initial robot position" 
    )

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

    odom_broadcast = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='map_to_odom_broadcaster',
        arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']
    )

    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters = [{'robot_description': Command(['xacro ', default_model_path])}]
    )

    controller_manager = Node(
        package='controller_manager',
        executable='ros2_control_node',
        output='screen',
        parameters = [params]
    )

    joint_state_broadcaster = Node(
        package='controller_manager',
        executable='spawner',
        output='screen',
        arguments= ["joint_state_broadcaster"]
    )

    diffbot_base_controller = Node(
        package='controller_manager',
        executable='spawner',
        output='both',
        arguments= [
            "diffbot_base_controller",
            "-p",
            params,
            "--controller-ros-args",
            "-r diffbot_base_controller/cmd_vel:=/cmd_vel",
            "--controller-ros-args",
            "-r diffbot_base_controller/odom:=/odom"
        ]
    )

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', default_rviz_config_path],
        condition=IfCondition(visualize)
    )

    return LaunchDescription([
        starting_position_arg,
        visualize_arg,
        odom_broadcast,
        robot_state_publisher,
        controller_manager,
        joint_state_broadcaster,
        diffbot_base_controller,
        rviz_node
    ])