from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import GroupAction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterFile
from launch.substitutions import PythonExpression
from launch.conditions import IfCondition, UnlessCondition
from nav2_common.launch import RewrittenYaml


ARGUMENTS = [
    DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09']),
]

def generate_launch_description():
    robot = LaunchConfiguration('robot')

    remappings=[
          ('/tf', 'tf'),
          ('/tf_static', 'tf_static'),
          ('odom', 'icp_odom'),
          ('scan_cloud', '/lidar_filtered')]
    
    # Directories
    pkg_localization = get_package_share_directory(
        'localization_pkg')
    
    rtabmap_params_file = PathJoinSubstitution([
        pkg_localization,
        'config',
        PythonExpression(["'rtabmap_' + '", robot, "' + '.yaml'"])])
    param_substitutions = {
        'use_sim_time': 'true'
    }
    configured_params = ParameterFile(
        RewrittenYaml(
            source_file=rtabmap_params_file,
            param_rewrites=param_substitutions,
            convert_types=True),
        allow_substs=True)
    
    load_nodes = GroupAction(
        actions=[
            Node(
                package='rtabmap_odom',
                executable='icp_odometry',
                name='icp_odometry',
                output='screen',
                parameters=[configured_params, {'use_sim_time': True}],
                remappings=remappings,
            )
        ],
    )

    # Create launch description and add actions
    ld = LaunchDescription(ARGUMENTS)
    ld.add_action(load_nodes)
    
    return ld