runtime/rbs_skill_servers/launch/skills.launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from rbs_launch_utils.launch_common import load_yaml


def launch_setup(context, *args, **kwargs):
    robot_description_decl = LaunchConfiguration("robot_description")
    robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
    robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
    use_sim_time = LaunchConfiguration("use_sim_time")
    with_gripper_condition = LaunchConfiguration("with_gripper_condition")
    points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
    robot_description = {"robot_description": robot_description_decl}
    robot_description_semantic = {
        "robot_description_semantic": robot_description_semantic_decl
    }
    namespace = LaunchConfiguration("namespace")

    points_params = load_yaml("rbs_skill_servers", "config/gripperPositions.yaml")

    kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")

    robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}

    move_topose_action_server = Node(
        package="rbs_skill_servers",
        executable="move_topose_action_server",
        namespace=namespace,
        parameters=[
            robot_description,
            robot_description_semantic,
            robot_description_kinematics,
            {"use_sim_time": use_sim_time},
        ],
    )

    gripper_control_node = Node(
        package="rbs_skill_servers",
        executable="gripper_control_action_server",
        namespace=namespace,
        parameters=[
            robot_description,
            robot_description_semantic,
            robot_description_kinematics,
            {"use_sim_time": use_sim_time},
        ],
        condition=IfCondition(with_gripper_condition),
    )

    move_cartesian_path_action_server = Node(
        package="rbs_skill_servers",
        executable="move_cartesian_path_action_server",
        namespace=namespace,
        parameters=[
            robot_description,
            robot_description_semantic,
            robot_description_kinematics,
            {"use_sim_time": use_sim_time},
        ],
    )

    cartesian_move_to_pose_action_server = Node(
        package="rbs_skill_servers",
        executable="move_to_pose.py",
        namespace=namespace,
        parameters=[{"use_sim_time": use_sim_time}, {"robot_name": namespace}],
    )

    move_joint_state_action_server = Node(
        package="rbs_skill_servers",
        executable="move_to_joint_states_action_server",
        namespace=namespace,
        parameters=[
            robot_description,
            robot_description_semantic,
            robot_description_kinematics,
            {"use_sim_time": use_sim_time},
        ],
    )

    assembly_config = Node(
        package="rbs_utils",
        executable="assembly_config_service.py",
        namespace=namespace,
        output="screen",
    )

    nodes_to_start = [
        assembly_config,
        move_topose_action_server,
        gripper_control_node,
        move_cartesian_path_action_server,
        move_joint_state_action_server,
        cartesian_move_to_pose_action_server,
        # grasp_pose_loader
    ]
    return nodes_to_start


def generate_launch_description():
    declared_arguments = []

    declared_arguments.append(
        DeclareLaunchArgument(
            "robot_description",
            default_value="",
            description="robot description string",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument("robot_description_semantic", default_value="")
    )
    declared_arguments.append(
        DeclareLaunchArgument("robot_description_kinematics", default_value="")
    )
    declared_arguments.append(DeclareLaunchArgument("use_sim_time", default_value=""))
    declared_arguments.append(
        DeclareLaunchArgument("with_gripper_condition", default_value="")
    )
    declared_arguments.append(
        DeclareLaunchArgument("points_params_filepath", default_value="")
    )
    declared_arguments.append(DeclareLaunchArgument("namespace", default_value=""))

    return LaunchDescription(
        declared_arguments + [OpaqueFunction(function=launch_setup)]
    )