from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_move_group_launch
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    moveit_config = MoveItConfigsBuilder(
        "FHrobot", package_name="dual_arm_moveit_action_server"
    ).to_moveit_configs()

    move_group_launch = generate_move_group_launch(moveit_config)
    rviz_config = LaunchConfiguration("rviz_config")
    rviz_config_arg = DeclareLaunchArgument(
        "rviz_config",
        default_value=str(moveit_config.package_path / "config/moveit.rviz"),
    )

    controllers_yaml = str(moveit_config.package_path / "config/ros2_controllers.yaml")

    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        output="screen",
        parameters=[
            moveit_config.robot_description,
            controllers_yaml,
        ],
    )

    rsp_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="screen",
        parameters=[moveit_config.robot_description],
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
        output="screen",
    )

    leftarm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["leftarm_controller", "--controller-manager", "/controller_manager"],
        output="screen",
    )

    rightarm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["rightarm_controller", "--controller-manager", "/controller_manager"],
        output="screen",
    )

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        output="log",
        arguments=["-d", rviz_config],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.planning_pipelines,
            moveit_config.robot_description_kinematics,
            moveit_config.joint_limits,
        ],
    )

    action_server_node = Node(
        package="dual_arm_moveit_action_server",
        executable="dual_arm_moveit_action_server",
        output="screen",
        parameters=[
            moveit_config.to_dict(),
            {
                "left_group_name": "LeftArm",
                "right_group_name": "RightArm",
                "planning_time": 5.0,
                "acceleration_scaling": 0.5,
                "cartesian_step": 0.005,
                "cartesian_jump_threshold": 0.1,
                "cartesian_min_fraction": 0.5,
            },
        ],
    )

    return LaunchDescription(
        [
            rsp_node,
            ros2_control_node,
            joint_state_broadcaster_spawner,
            leftarm_controller_spawner,
            rightarm_controller_spawner,
            move_group_launch,
            rviz_config_arg,
            rviz_node,
            action_server_node,
        ]
    )

