from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction
from launch.event_handlers import OnProcessStart
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

from pathlib import Path

def generate_launch_description():

    basedir = FindPackageShare("mg_wheel_interface")

    urdfDescription = PathJoinSubstitution([basedir, "assets/mg_base.urdf"])
    yamlControllers = PathJoinSubstitution([basedir, "assets/mg_base_controllers.yaml"])

    urdfDescription = Command([
        "cat ",
        urdfDescription
    ])

    return LaunchDescription([
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            output="screen",
            parameters=[{"robot_description": urdfDescription}]
        ),
        Node(
            package="controller_manager",
            executable="ros2_control_node",
            parameters=[yamlControllers],
            output="screen"
        ),
        Node(
            package='controller_manager',
            executable='spawner',
            output="screen",
            arguments=["joint_state_publisher"]
        ),
        Node(
            package="controller_manager",
            executable="spawner",
            arguments=["diffdrive_controller", "--param-file", yamlControllers],
        ) 
    ])