refactor: remove command_mode from configuration and related files

2026-05-25 12:30:59 +10:00
parent e60402c8f2
commit b560fcf229
13 changed files with 35 additions and 299 deletions
@@ -1,160 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    EmitEvent,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
-from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
-from launch.events import Shutdown
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-from moveit_configs_utils import MoveItConfigsBuilder
-
-from iiwa_utils import converter, setting_loader
-
-
-def _runtime_setup(context, *args, **kwatgs):
-    setup = []
-
-    settings = setting_loader.build_settings(
-        settings_path=LaunchConfiguration("setting").perform(context), check_files=True
-    )
-
-    robot_description = converter.load_robot_description(
-        model_path=settings.robot.description,
-        robot_name=settings.robot.name,
-        xacro_args={
-            "initial_positions_file": settings.controller.moveit.initial_positions
-        },
-    )
-
-    rsp_node = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        name="robot_state_publisher",
-        output="screen",
-        parameters=[{"robot_description": robot_description, "use_sim_time": True}],
-    )
-
-    webots_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("iiwa_bringup"),
-                    "launch",
-                    "supported",
-                    "webots_spawn.launch.py",
-                ]
-            )
-        ),
-        launch_arguments={
-            "robot_name": str(settings.robot.name),
-            "description": str(settings.robot.description),
-            "world": str(settings.digital_twin.webots.world),
-            "transform": str(settings.digital_twin.webots.transform),
-            "rotation": str(settings.digital_twin.webots.rotation),
-            "controller_timer": str(settings.digital_twin.webots.controller_timer),
-            "controller": str(settings.controller.controller_path),
-            "initial_positions_file": str(settings.controller.moveit.initial_positions),
-        }.items(),
-    )
-
-    moveit_configs = (
-        MoveItConfigsBuilder("iiwa7", package_name="iiwa_config")
-        .robot_description(
-            file_path=settings.robot.description,
-            mappings={
-                "initial_positions_file": settings.controller.moveit.initial_positions
-            },
-        )
-        .robot_description_semantic(file_path=settings.controller.moveit.srdf)
-        .robot_description_kinematics(file_path=settings.controller.moveit.kinematics)
-        .joint_limits(file_path=settings.controller.moveit.joint_limits)
-        .pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits)
-        .trajectory_execution(file_path=settings.controller.moveit.moveit_controllers)
-        .moveit_cpp(file_path=settings.controller.moveit.moveit_cpp)
-        .to_moveit_configs()
-    )
-
-    move_group = Node(
-        package="moveit_ros_move_group",
-        executable="move_group",
-        output="screen",
-        parameters=[
-            moveit_configs.to_dict(),
-            {"robot_description": robot_description},
-            {"use_sim_time": True},
-        ],
-    )
-
-    # TODO: не забудь поменять правильное название и имя пакета
-    # moveit_py_node = Node(
-    #     # name="motion_planning_node",
-    #     package="iiwa_planning",
-    #     executable="motion_planning",
-    #     output="both",
-    #     parameters=[moveit_configs.to_dict()],
-    # )
-
-
-    rviz_launch = Node(
-        condition=IfCondition(LaunchConfiguration("rviz")),
-        package="rviz2",
-        executable="rviz2",
-        name="rviz2",
-        arguments=["-d", settings.digital_twin.rviz.config],
-        output="log",
-        parameters=[
-            moveit_configs.robot_description,
-            moveit_configs.robot_description_semantic,
-            moveit_configs.planning_pipelines,
-            moveit_configs.planning_scene_monitor,
-            {"use_sim_time": True},
-        ],
-    )
-
-    shutdown_on_rviz_exit = RegisterEventHandler(
-        OnProcessExit(target_action=rviz_launch, on_exit=[EmitEvent(event=Shutdown())])
-    )
-
-    setup += [
-        rsp_node,
-        webots_launch,
-        move_group,
-        # moveit_py_node,
-        rviz_launch,
-        shutdown_on_rviz_exit,
-    ]
-
-    return setup
-
-
-def generate_launch_description():
-    declare_rviz = DeclareLaunchArgument(
-        name="rviz",
-        default_value="0",
-        description="If true|1|yes then launch RViz/MoveIt branch (instead of controllers branch)",
-    )
-
-    declacre_setting = DeclareLaunchArgument(
-        name="setting",
-        default_value=PathJoinSubstitution(
-            [FindPackageShare("iiwa_config"), "config", "setting.yaml"]
-        ),
-        description="Absolute path to settings file",
-    )
-
-    runtime_setup = OpaqueFunction(function=_runtime_setup)
-
-    return LaunchDescription(
-        [
-            declare_rviz,
-            declacre_setting,
-            runtime_setup,
-        ]
-    )
@@ -65,7 +65,6 @@ def _runtime_setup(context, *args, **kwargs):
            "robot_ip": settings.robot.ip,
            "fri_port": str(settings.robot.port),
            "simulate": "false",
-            "command_mode": settings.robot.command_mode,
            "joint_position_tau": str(settings.robot.joint_position_tau),
        }
        use_sim_time = False
@@ -168,7 +167,6 @@ def _runtime_setup(context, *args, **kwargs):
            "transform": str(settings.digital_twin.webots.transform),
            "rotation": str(settings.digital_twin.webots.rotation),
            "simulate": "false",
-            "command_mode": settings.robot.command_mode,
            "controller_timer": str(settings.digital_twin.webots.controller_timer),
            "fri_cycle_ms": str(settings.robot.fri_cycle_ms),
            "joint_position_tau": str(settings.robot.joint_position_tau),
@@ -19,7 +19,6 @@ def _setup_controllers(context, *args, **kwargs):
    controller_timer = LaunchConfiguration("controller_timer").perform(context)
    controller_path = LaunchConfiguration("controller_path").perform(context)
    simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes")
-    command_mode = LaunchConfiguration("command_mode").perform(context)
    controller = LaunchConfiguration("controller").perform(context)  # "jtc" | "forward"
    fri_cycle_ms = int(LaunchConfiguration("fri_cycle_ms").perform(context))
    joint_position_tau = LaunchConfiguration("joint_position_tau").perform(context)
@@ -65,18 +64,10 @@ def _setup_controllers(context, *args, **kwargs):
            parameters=[{"use_sim_time": True}],
        )

-        torque_controller_spawner = Node(
-            package="controller_manager",
-            executable="spawner",
-            output="screen",
-            arguments=["iiwa_arm_torque_controller", "--inactive"] + tmo,
-            parameters=[{"use_sim_time": True}]
-        )
-
        jtc_after_jsb = RegisterEventHandler(
            OnProcessExit(
                target_action=jsb,
-                on_exit=[jtc, torque_controller_spawner],
+                on_exit=[jtc],
            )
        )

@@ -107,9 +98,8 @@ def _setup_controllers(context, *args, **kwargs):

        cm = ["--controller-manager", "/controller_manager"]

-        # JTC: активен если controller=jtc (и command_mode=position), иначе --inactive
        jtc_args = ["iiwa_arm_controller"] + cm
-        if command_mode == "torque" or controller == "forward":
+        if controller == "forward":
            jtc_args += ["--inactive"]

        # ForwardCommandController: активен если controller=forward, иначе --inactive
@@ -117,11 +107,6 @@ def _setup_controllers(context, *args, **kwargs):
        if controller != "forward":
            forward_args += ["--inactive"]

-        # TorqueController: активен если command_mode=torque и controller=jtc
-        torque_args = ["iiwa_arm_torque_controller"] + cm
-        if not (command_mode == "torque" and controller == "jtc"):
-            torque_args += ["--inactive"]
-
        jtc = Node(
            package="controller_manager",
            executable="spawner",
@@ -136,17 +121,10 @@ def _setup_controllers(context, *args, **kwargs):
            arguments=forward_args,
        )

-        torque_controller = Node(
-            package="controller_manager",
-            executable="spawner",
-            output="screen",
-            arguments=torque_args,
-        )
-
        jtc_after_jsb = RegisterEventHandler(
            OnProcessExit(
                target_action=jsb,
-                on_exit=[jtc, forward_controller, torque_controller],
+                on_exit=[jtc, forward_controller],
            )
        )

@@ -159,7 +137,6 @@ def _setup_controllers(context, *args, **kwargs):

 def generate_launch_description():
    return LaunchDescription([
-        DeclareLaunchArgument("command_mode", default_value="position"),
        DeclareLaunchArgument("fri_cycle_ms", default_value="5"),
        DeclareLaunchArgument("joint_position_tau", default_value="0.04"),
        DeclareLaunchArgument("controller", default_value="jtc"),