import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    pkg_dir = get_package_share_directory("planner")
    params_file = LaunchConfiguration(
        "params_file",
        default=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"),
    )
    use_sim_time = LaunchConfiguration("use_sim_time", default="false")

    planner_node = Node(
        package="planner",
        executable="grid_astar_theta_node",
        name="grid_astar_theta_planner",
        output="screen",
        parameters=[
            params_file,
            {"use_sim_time": use_sim_time},
        ],
    )

    return LaunchDescription(
        [
            DeclareLaunchArgument(
                "params_file",
                default_value=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"),
                description="Parameters for lightweight Ackermann Hybrid A* planner.",
            ),
            DeclareLaunchArgument(
                "use_sim_time",
                default_value="false",
                description="Use simulation clock.",
            ),
            planner_node,
        ]
    )
