cnc-graver-assist/rbs_mill_assist/launch/moveit.launch.py

from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch_ros.substitutions import FindPackage, FindPackageShare
from launch.substitutions import (
    Command,
    FindExecutable,
    LaunchConfiguration,
    PathJoinSubstitution,
)
from rbs_launch_utils.launch_common import (
    get_package_share_directory,
    load_yaml,
    load_yaml_abs,
)
import os
from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():
    declared_arguments = []
    declared_arguments.append(
        DeclareLaunchArgument(
            "robot_description",
            default_value="''",
            description="robot description param",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "robot_description_semantic",
            default_value="''",
            description="robot description semantic param",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "robot_description_kinematics",
            default_value="''",
            description="robot description kinematics param",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "moveit_config_package",
            default_value="rbs_arm",
            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
        is not set, it enables use of a custom moveit config.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "moveit_config_file",
            default_value="rbs_arm.srdf.xacro",
            description="MoveIt SRDF/XACRO description file with the robot.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "use_sim_time",
            default_value="true",
            description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tf_prefix",
            default_value="''",
            description="tf_prefix for robot links",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "namespace",
            default_value="",
            description="Namespace for move_group_node",
        )
    )

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


def launch_setup(context, *args, **kwargs):
    use_sim_time = LaunchConfiguration("use_sim_time")
    robot_description_content = LaunchConfiguration("robot_description").perform(
        context
    )
    robot_description_semantic_content = LaunchConfiguration(
        "robot_description_semantic"
    ).perform(context)
    robot_description_kinematics_filepath = LaunchConfiguration(
        "robot_description_kinematics"
    ).perform(context)
    namespace = LaunchConfiguration("namespace").perform(context)
    moveit_config_package = LaunchConfiguration("moveit_config_package").perform(
        context
    )

    robot_description = {"robot_description": robot_description_content}
    robot_description_semantic = {
        "robot_description_semantic": robot_description_semantic_content
    }
    robot_description_kinematics = {
        "robot_description_kinematics": load_yaml_abs(
            robot_description_kinematics_filepath
        )
    }
    use_sim_time = {"use_sim_time": use_sim_time}

    # Planning Configuration
    ompl_planning_pipeline_config = {
        "default_planning_pipeline": "ompl",
        "planning_pipelines": ["ompl"],
        "ompl": {
            "planning_plugin": "ompl_interface/OMPLPlanner",
            "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
            "start_state_max_bounds_error": 0.1,
        },
    }
    ompl_planning_yaml = load_yaml(moveit_config_package, "moveit/ompl_planning.yaml")
    ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml)

    moveit_controllers = load_yaml(
        moveit_config_package, "moveit/moveit_controllers.yaml"
    )

    # moveit_controllers = {
    #     "moveit_simple_controller_manager":
    #         controllers_yaml,
    #     "moveit_controller_manager":
    #     "moveit_simple_controller_manager/MoveItSimpleControllerManager",
    # }

    moveit_config = (
        MoveItConfigsBuilder(robot_name="rbs_arm", package_name="rbs_mill_assist")
        .robot_description(file_path="urdf/rbs_arm.xacro")
        .trajectory_execution(file_path="moveit/moveit_controllers.yaml")
        .joint_limits("moveit/joint_limits.yaml")
        .pilz_cartesian_limits("moveit/pilz_cartesian_limits.yaml")
        .robot_description_kinematics("moveit/kinematics.yaml")
        .robot_description_semantic("srdf/rbs_arm.srdf")
        .moveit_cpp("moveit/moveit_cpp.yaml")
        .planning_scene_monitor(
            publish_geometry_updates=True,
            publish_planning_scene=True,
            publish_robot_description=True,
            publish_robot_description_semantic=True,
        )
        .to_moveit_configs()
    )


    # trajectory_execution = {
    #     "moveit_manage_controllers": False,
    #     "trajectory_execution.allowed_execution_duration_scaling": 1.2,
    #     "trajectory_execution.allowed_goal_duration_margin": 0.5,
    #     "trajectory_execution.allowed_start_tolerance": 0.01,
    # }

    planning_scene_monitor_parameters = {
        "publish_planning_scene": True,
        "publish_geometry_updates": True,
        "publish_state_updates": True,
        "publish_transforms_updates": True,
    }

    robot_description_planning = {
        "robot_description_planning": load_yaml(
            moveit_config_package,
            os.path.join("moveit", "joint_limits.yaml"),
        )
    }

    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        namespace=namespace,
        parameters=[
            robot_description,
            robot_description_semantic,
            robot_description_kinematics,
            ompl_planning_pipeline_config,
            # trajectory_execution,
            moveit_controllers,
            planning_scene_monitor_parameters,
            use_sim_time,
            robot_description_planning,
            # moveit_config.to_dict()
        ],
    )
    planning_scene_publisher = Node(
        package="rbs_mill_assist",
        executable="planning_scene_publisher",
        parameters=[moveit_config.to_dict(), {"use_sim_time": True}],
    )

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        # arguments=["-d", rviz_full_config],
        parameters=[
            # robot_description,
            # robot_description_semantic,
            # robot_description_kinematics,
            # robot_description_planning,
            # ompl_planning_pipeline_config,
            # # trajectory_execution,
            # moveit_controllers,
            # planning_scene_monitor_parameters,
            {"use_sim_time": True},
            # moveit_config.robot_description,
            # moveit_config.robot_description_semantic,
            # moveit_config.robot_description_kinematics,
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.planning_pipelines,
            moveit_config.planning_scene_monitor,
            moveit_config.robot_description_kinematics,
            moveit_config.joint_limits,
        ],
    )

    nodes_to_start = [
        move_group_node,
        planning_scene_publisher,
        rviz_node,
    ]

    return nodes_to_start