Add motion planning actions and services for iiwa robot

- Implemented action servers for MoveToPose and MoveToJoints.
- Added service for MoveToNamedPose and stop action.
- Updated README with usage examples for new actions.
- Enhanced planning configurations in YAML and Python files.
- Removed deprecated motion planning scripts.

README.md

@@ -46,6 +46,22 @@ LC_ALL=C ros2 launch iiwa_moveit demo...
 
 sudo pip3 install transforms3d --break-system-packages
 ```
+Отправка робота в точку:
+```
+ros2 action send_goal /iiwa/move_to_pose iiwa_msgs/action/MoveToPose \
+  "{x: 0.5, y: 0.0, z: 0.5, a: 3.14, b: 0, c: 0, speed: 0.1, planner: 'ptp'}"
+
+ros2 action send_goal --feedback /iiwa/move_to_joints iiwa_msgs/action/MoveToJoints \
+  "{joints: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0], speed: 0.4}"
+
+ros2 service call /iiwa/move_to_named iiwa_msgs/srv/MoveToNamedPose \
+  "{name: 'home', speed: 0.5}"
+
+ros2 service call /iiwa/move_to_named iiwa_msgs/srv/MoveToNamedPose \
+  "{name: 'work', speed: 0.3}"
+
+ros2 service call /iiwa/stop std_srvs/srv/Trigger "{}"
+```
 
 Спавн объекта:
 ```bash

src/iiwa_bringup/launch/iiwa.launch.py

@@ -219,6 +219,13 @@ def _runtime_setup(context, *args, **kwargs):
             moveit_configs.to_dict(),
             {"robot_description": robot_description},
             {"use_sim_time": use_sim_time},
+            {
+                "pose_link": settings.planning.pose_link,
+                "planning_group": settings.planning.planning_group,
+                "default_frame": settings.planning.default_frame,
+                "default_planner": settings.planning.default_planner,
+                "planning_attempts": settings.planning.planning_attempts,
+            },
         ],
     )
 

src/iiwa_config/config/moveit/kinematics.yaml

@@ -1,4 +1,5 @@
 iiwa_arm:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-  kinematics_solver_search_resolution: 0.0050000000000000001
+  kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.0050000000000000001
+  kinematics_solver_timeout: 0.005
+  kinematics_solver_attempts: 3

src/iiwa_config/config/moveit/moveit_cpp.yaml

@@ -25,7 +25,7 @@ ompl_rrtc:  # Namespace for individual plan request
     max_acceleration_scaling_factor: 1.0  # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
     planning_time: 1.0  # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned
 
-pilz_lin:
+pilz_ptp:
   plan_request_params:
     planning_attempts: 1
     planning_pipeline: pilz_industrial_motion_planner
@@ -34,6 +34,15 @@ pilz_lin:
     max_acceleration_scaling_factor: 1.0
     planning_time: 0.8
 
+pilz_lin:
+  plan_request_params:
+    planning_attempts: 1
+    planning_pipeline: pilz_industrial_motion_planner
+    planner_id: "LIN"
+    max_velocity_scaling_factor: 1.0
+    max_acceleration_scaling_factor: 1.0
+    planning_time: 0.8
+
 chomp_planner:
   plan_request_params:
     planning_attempts: 1

src/iiwa_config/config/setting.yaml

@@ -33,6 +33,13 @@ controller:
     moveit_cpp: pkg://iiwa_config/config/moveit/moveit_cpp.yaml
 
 
+planning:
+  pose_link: "link_ee"        # TCP-линк для декартовых целей
+  planning_group: "iiwa_arm"  # Группа планирования из SRDF
+  default_frame: "base_link"  # Система отсчёта по умолчанию
+  default_planner: "ompl"     # Планировщик по умолчанию
+  planning_attempts: 3        # Число попыток планирования
+
 foxglove:
   enabled: true  # Запускать ли foxglove_bridge вместе с роботом
   port: 8765 # WebSocket-порт, к которому подключается Foxglove Studio (по умолчанию 8765)

src/iiwa_msgs/CMakeLists.txt

@@ -4,10 +4,14 @@ project(iiwa_msgs)
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(action_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/MoveToPose.srv"
-  DEPENDENCIES geometry_msgs
+  "srv/MoveToNamedPose.srv"
+  "action/MoveToPose.action"
+  "action/MoveToJoints.action"
+  DEPENDENCIES geometry_msgs action_msgs
 )
 
 ament_export_dependencies(rosidl_default_runtime)

src/iiwa_msgs/action/MoveToJoints.action

@@ -0,0 +1,11 @@
+# Goal — 7 joint positions in radians
+float64[7] joints
+# Velocity scaling 0.0–1.0
+float32 speed
+---
+# Result
+bool success
+string message
+---
+# Feedback
+string state  # "planning" | "executing"

src/iiwa_msgs/action/MoveToPose.action

@@ -0,0 +1,21 @@
+# Goal
+float64 x
+float64 y
+float64 z
+# ZYX Euler in radians (A=yaw, B=pitch, C=roll — KUKA ABC convention)
+float64 a
+float64 b
+float64 c
+# Velocity scaling 0.0–1.0
+float32 speed
+# Planner: ompl | ptp | lin | circ | chomp (default: ompl)
+string planner
+# Reference frame (default: base_link)
+string frame_id
+---
+# Result
+bool success
+string message
+---
+# Feedback
+string state  # "planning" | "executing"

src/iiwa_msgs/package.xml

@@ -11,6 +11,7 @@
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <depend>geometry_msgs</depend>
+  <depend>action_msgs</depend>
   <depend>rosidl_default_runtime</depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>

src/iiwa_msgs/srv/MoveToNamedPose.srv

@@ -0,0 +1,7 @@
+# Named state from SRDF (e.g. "home", "work")
+string name
+# Velocity scaling 0.0–1.0
+float32 speed
+---
+bool success
+string message

src/iiwa_msgs/srv/MoveToPose.srv

@@ -1,4 +1,17 @@
-geometry_msgs/PoseStamped pose
+# Position in metres
+float64 x
+float64 y
+float64 z
+# Orientation as ZYX Euler angles in degrees (KUKA ABC convention: A=yaw, B=pitch, C=roll)
+float64 a
+float64 b
+float64 c
+# Velocity scaling factor 0–100 (%)
+float32 speed
+# Planner: "ompl" | "ptp" | "lin" | "chomp" (default: "ompl")
+string planner
+# Reference frame, defaults to "base_link" if empty
+string frame_id
 ---
 bool success
 string message

src/iiwa_planning/CMakeLists.txt

@@ -37,13 +37,6 @@ install(TARGETS
   DESTINATION lib/${PROJECT_NAME}
 )
 
-install(PROGRAMS
-  scripts/motion_planning_test.py
-  # scripts/motion_planning.py
-  DESTINATION lib/${PROJECT_NAME}
-  RENAME motion_planning
-)
-
 install(PROGRAMS
   scripts/move_to_pose_server.py
   DESTINATION lib/${PROJECT_NAME}

src/iiwa_planning/scripts/motion_planning.py

@@ -1,129 +0,0 @@
-#!/usr/bin/env python3
-
-import rclpy
-from rclpy.node import Node
-from moveit.core.robot_state import RobotState
-from moveit.planning import MoveItPy, PlanningComponent
-from geometry_msgs.msg import PoseStamped
-from rclpy.executors import ExternalShutdownException
-from tf_transformations import quaternion_from_euler
-
-
-class MotionPlaning(Node):
-    def __init__(self):
-        super().__init__("motion_planning_node")
-
-        self.moveit = MoveItPy(node_name="motion_planning_node")
-        self.robot_arm: PlanningComponent = self.moveit.get_planning_component("iiwa_arm")
-        self.planning_scene_monitor = self.moveit.get_planning_scene_monitor()
-
-        
-        self.get_logger().warn("***********************************************************")
-        self.get_logger().info("MotionPlanning node is ready...")
-        self.get_logger().warn("***********************************************************")
-        # self.robot_arm.set_start_state(configuration_name="home")
-        # self.robot_arm.set_goal_state(configuration_name="work")
-        # self.plan_end_execute()
-
-        self.timer = self.create_timer(5.0, self._run_once)
-
-    def _run_once(self):
-        self.timer.cancel()
-        self.handle_plan_execute_pose(0.677, 0.25, 0.21, 3.14, 0, 3.14)
-
-
-    def handle_plan_execute_pose(self, x, y, z, a, b, c):
-        
-        pose_goal = PoseStamped()
-        pose_goal.header.frame_id="base_link"
-        pose_goal.header.stamp = self.get_clock().now().to_msg()
-        pose_goal.pose.position.x=x
-        pose_goal.pose.position.y=y
-        pose_goal.pose.position.z=z
-
-        xx, yy, zz, w = quaternion_from_euler(a, b, c)
-
-        pose_goal.pose.orientation.x=xx
-        pose_goal.pose.orientation.y=yy
-        pose_goal.pose.orientation.z=zz
-        pose_goal.pose.orientation.w=w
-
-        # Check collisions
-        with self.planning_scene_monitor.read_only() as scene:
-            
-            robot_state = scene.current_state
-            original_joint_positions = robot_state.get_joint_group_positions("iiwa_arm")
-
-            ok_ik = robot_state.set_from_ik("iiwa_arm", pose_goal.pose, "link_ee")
-            robot_state.update() 
-
-            if not ok_ik:
-                self.get_logger().warn("***********************************************************")
-                self.get_logger().error("IK failed -> abort")
-                self.get_logger().warn("***********************************************************")
-                return
-
-            robot_collision_status = scene.is_state_colliding(
-                robot_state=robot_state, 
-                joint_model_group_name="iiwa_arm", 
-                verbose=True
-            )
-            if robot_collision_status:
-                self.get_logger().warn("***********************************************************")
-                self.get_logger().error("Goal in collision -> abort")
-                self.get_logger().warn("***********************************************************")
-                return
-
-            self.get_logger().warn("***********************************************************")
-            self.get_logger().info(f"\nRobot is in collision: {robot_collision_status}\n")
-            self.get_logger().warn("***********************************************************")
-
-            robot_state.set_joint_group_positions(
-                "iiwa_arm",
-                original_joint_positions,
-            )
-            robot_state.update() 
-
-        # получить текущее состояние как изначальное для перемещения
-        self.robot_arm.set_start_state_to_current_state()
-        # self.robot_arm.set_start_state(configuration_name="home")
-        # self.robot_arm.set_goal_state(configuration_name="work")
-
-        self.robot_arm.set_goal_state(
-            pose_stamped_msg=pose_goal, 
-            pose_link="link_ee"
-        )
-        self.plan_end_execute()
-
-
-    def plan_end_execute(self):
-        self.get_logger().warn("***********************************************************")
-        self.get_logger().info("Planning trajectory")
-        self.get_logger().warn("***********************************************************")
-
-        plan_result = self.robot_arm.plan()
-        if plan_result:
-            self.get_logger().warn("***********************************************************")
-            self.get_logger().info("Executing plan")
-            self.get_logger().warn("***********************************************************")
-            robot_trajectory = plan_result.trajectory
-            self.moveit.execute(robot_trajectory, controllers=[])
-        else:
-            self.get_logger().warn("***********************************************************")
-            self.get_logger().info("Planning failed")
-            self.get_logger().warn("***********************************************************")
-        
-
-def main(args=None):
-    try:
-        rclpy.init(args=args)
-        node = MotionPlaning()
-        rclpy.spin(node=node)
-        node.destroy_node()
-    except (KeyboardInterrupt, ExternalShutdownException):
-        pass
-    finally:
-        rclpy.shutdown()
-
-if __name__ == "__main__":
-    main()

src/iiwa_planning/scripts/motion_planning_test.py

@@ -1,89 +0,0 @@
-#!/usr/bin/env python3
-import time
-
-# generic ros libraries
-import rclpy
-from moveit.core.robot_state import RobotState
-from moveit.planning import MoveItPy
-from rclpy.impl.rcutils_logger import RcutilsLogger
-from rclpy.logging import get_logger
-
-
-def plan_and_execute(robot: MoveItPy,
-                     planning_component,
-                     logger: RcutilsLogger,
-                     single_plan_parameters=None,
-                     multi_plan_parameters=None,
-                     sleep_time=0.0):
-    logger.info("Planning trajectory")
-
-    if multi_plan_parameters is not None:
-        plan_result = planning_component.plan(multi_plan_parameters=multi_plan_parameters)
-    elif single_plan_parameters is not None:
-        plan_result = planning_component.plan(
-            single_plan_parameters=single_plan_parameters
-        )
-    else:
-        plan_result = planning_component.plan()
-
-    # execute the plan
-    if plan_result:
-        logger.info("Executing plan")
-        robot_trajectory = plan_result.trajectory
-        robot.execute(robot_trajectory, controllers=[])
-    else:
-        logger.error("Planning failed")
-
-    time.sleep(sleep_time)
-
-
-def main():
-    rclpy.init()
-    logger = get_logger("moveit_py.pose_goal")
-
-    iiwa = MoveItPy(node_name="moveit_py")
-    iiwa_arm = iiwa.get_planning_component("iiwa_arm")
-    logger.info("MoveItPy instance created")
-
-    iiwa_arm.set_start_state(configuration_name="ready")
-    iiwa_arm.set_goal_state(configuration_name="extended")
-
-    plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
-
-    robot_model = iiwa.get_robot_model()
-    robot_state = RobotState(robot_model)
-
-    # randomize the robot state
-    robot_state.set_to_random_positions()
-
-    # set plan start state to current state
-    iiwa_arm.set_start_state_to_current_state()
-
-    # set goal state to the initialized robot state
-    logger.info("Set goal state to the initialized robot state")
-    iiwa_arm.set_goal_state(robot_state=robot_state)
-
-    # plan to goal
-    plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
-
-    # set plan start state to current state
-    iiwa_arm.set_start_state_to_current_state()
-
-    # set pose goal with PoseStamped message
-    from geometry_msgs.msg import PoseStamped
-
-    pose_goal = PoseStamped()
-    pose_goal.header.frame_id = "base_link"
-    pose_goal.pose.orientation.w = 1.0
-    pose_goal.pose.position.x = 0.28
-    pose_goal.pose.position.y = -0.2
-    pose_goal.pose.position.z = 0.5
-    iiwa_arm.set_goal_state(pose_stamped_msg=pose_goal, 
-                            pose_link="patron")
-
-    # plan to goal
-    plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
-
-
-if __name__ == "__main__":
-    main()

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -1,67 +1,290 @@
 #!/usr/bin/env python3
 
+import threading
+
 import rclpy
 from rclpy.node import Node
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.executors import MultiThreadedExecutor, ExternalShutdownException
-from moveit.planning import MoveItPy, PlanningComponent
+
-from iiwa_msgs.srv import MoveToPose
+from geometry_msgs.msg import PoseStamped
+from scipy.spatial.transform import Rotation
+from std_srvs.srv import Trigger
+
+from moveit.planning import MoveItPy, PlanningComponent, PlanRequestParameters
+from moveit.core.robot_state import RobotState
+
+from iiwa_msgs.action import MoveToPose, MoveToJoints
+from iiwa_msgs.srv import MoveToNamedPose
 
 
-POSE_LINK = "link_ee"
+# Таблица планировщиков: имя → (pipeline, planner_id, время планирования в секундах)
-PLANNING_GROUP = "iiwa_arm"
+PLANNERS = {
+    "ompl":  ("ompl",                           "RRTConnectkConfigDefault", 10.0),
+    "ptp":   ("pilz_industrial_motion_planner",  "PTP",                      2.0),
+    "lin":   ("pilz_industrial_motion_planner",  "LIN",                      2.0),
+    "circ":  ("pilz_industrial_motion_planner",  "CIRC",                     2.0),
+    "chomp": ("chomp",                           "",                         10.0),
+}
+
+
+def _abc_to_quaternion(a: float, b: float, c: float):
+    """ZYX Euler (радианы, конвенция KUKA ABC) → (qx, qy, qz, qw)."""
+    return Rotation.from_euler("ZYX", [a, b, c], degrees=False).as_quat()
+
+
+class IiwaMotionServer(Node):
+    """Сервер управления движением манипулятора iiwa.
+
+    Предоставляет:
+      - action iiwa/move_to_pose    — перемещение в декартову позу
+      - action iiwa/move_to_joints  — перемещение по суставным координатам
+      - service iiwa/move_to_named  — перемещение в именованную позу из SRDF
+      - service iiwa/stop           — немедленная остановка движения
+    """
+
+    def __init__(self):
+        super().__init__("iiwa_motion_server")
+        self._setup_parameters()
+        self._setup_moveit()
+        self._setup_servers()
+
+        self.get_logger().info("Сервер движения iiwa запущен")
+        self.get_logger().info(f"  pose_link={self._pose_link}  group={self._planning_group}")
+
+    def _setup_parameters(self):
+        self.declare_parameter("pose_link", "link_ee")
+        self.declare_parameter("planning_group", "iiwa_arm")
+        self.declare_parameter("default_frame", "base_link")
+        self.declare_parameter("default_planner", "ompl")
+        self.declare_parameter("planning_attempts", 3)
+
+        self._pose_link = self.get_parameter("pose_link").value
+        self._planning_group = self.get_parameter("planning_group").value
+        self._default_frame = self.get_parameter("default_frame").value
+        self._default_planner = self.get_parameter("default_planner").value
+        self._planning_attempts = self.get_parameter("planning_attempts").value
+
+    def _setup_moveit(self):
+        self._moveit = MoveItPy(node_name="iiwa_motion_server")
+        self._arm: PlanningComponent = self._moveit.get_planning_component(self._planning_group)
+        self._robot_model = self._moveit.get_robot_model()
+
+    def _setup_servers(self):
+        cb = ReentrantCallbackGroup()
+
+        ActionServer(
+            self, MoveToPose, "iiwa/move_to_pose", self._execute_pose,
+            callback_group=cb,
+            goal_callback=lambda _: GoalResponse.ACCEPT,
+            cancel_callback=lambda _: CancelResponse.ACCEPT,
+        )
+        ActionServer(
+            self, MoveToJoints, "iiwa/move_to_joints", self._execute_joints,
+            callback_group=cb,
+            goal_callback=lambda _: GoalResponse.ACCEPT,
+            cancel_callback=lambda _: CancelResponse.ACCEPT,
+        )
+        self.create_service(MoveToNamedPose, "iiwa/move_to_named", self._handle_named, callback_group=cb)
+        self.create_service(Trigger, "iiwa/stop", self._handle_stop, callback_group=cb)
+
+    def _make_plan_params(self, pipeline: str, planner_id: str, plan_time: float, velocity_scale: float) -> PlanRequestParameters:
+        params = PlanRequestParameters(self._moveit, self._planning_group)
+        params.planning_pipeline = pipeline
+        params.planner_id = planner_id
+        params.planning_time = plan_time
+        params.planning_attempts = self._planning_attempts
+        params.max_velocity_scaling_factor = velocity_scale
+        params.max_acceleration_scaling_factor = velocity_scale
+        return params
+
+    def _plan_and_execute(self, plan_params: PlanRequestParameters, goal_handle):
+        """Планирует траекторию и выполняет её с поддержкой отмены.
+
+        Возвращает (True, msg) при успехе, (False, msg) при ошибке,
+        (None, 'canceled') если цель была отменена.
+        """
+        plan_result = self._arm.plan(single_plan_parameters=plan_params)
+        if not plan_result:
+            return False, "Планирование не удалось: поза недостижима или в столкновении"
+
+        if goal_handle.is_cancel_requested:
+            return None, "canceled"
+
+        done = threading.Event()
+        failed = threading.Event()
+
+        def do_execute():
+            try:
+                self._moveit.execute(plan_result.trajectory, controllers=[])
+            except Exception as exc:
+                self.get_logger().error(f"Ошибка выполнения траектории: {exc}")
+                failed.set()
+            finally:
+                done.set()
+
+        threading.Thread(target=do_execute, daemon=True).start()
+
+        while not done.wait(timeout=0.05):
+            if goal_handle.is_cancel_requested:
+                try:
+                    self._moveit.get_trajectory_execution_manager().stop_execution()
+                except Exception:
+                    pass
+                done.wait()
+                return None, "canceled"
+
+        if failed.is_set():
+            return False, "Выполнение траектории завершилось ошибкой"
+
+        return True, "Движение выполнено успешно"
+
+    def _finish_action(self, goal_handle, result, ok, msg):
+        """Устанавливает финальное состояние action goal и заполняет result."""
+        result.success = bool(ok)
+        result.message = msg
+        if ok is None:
+            goal_handle.canceled()
+        elif ok:
+            goal_handle.succeed()
+        else:
+            self.get_logger().error(msg)
+            goal_handle.abort()
+        return result
+
+    def _execute_pose(self, goal_handle):
+        req = goal_handle.request
+        feedback = MoveToPose.Feedback()
+        result = MoveToPose.Result()
+
+        velocity_scale = max(0.01, min(1.0, float(req.speed)))
+        planner_key = (req.planner or self._default_planner).lower()
+
+        if planner_key not in PLANNERS:
+            result.success = False
+            result.message = f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}"
+            self.get_logger().error(result.message)
+            goal_handle.abort()
+            return result
+
+        pipeline, planner_id, plan_time = PLANNERS[planner_key]
+
+        pose = PoseStamped()
+        pose.header.frame_id = req.frame_id or self._default_frame
+        pose.pose.position.x = req.x
+        pose.pose.position.y = req.y
+        pose.pose.position.z = req.z
+        qx, qy, qz, qw = _abc_to_quaternion(req.a, req.b, req.c)
+        pose.pose.orientation.x = qx
+        pose.pose.orientation.y = qy
+        pose.pose.orientation.z = qz
+        pose.pose.orientation.w = qw
+
+        self.get_logger().info(
+            f"[pose] xyz=({req.x:.3f}, {req.y:.3f}, {req.z:.3f})  "
+            f"abc=({req.a:.3f}, {req.b:.3f}, {req.c:.3f}) рад  "
+            f"speed={velocity_scale:.2f}  planner={planner_key}"
+        )
+
+        feedback.state = "planning"
+        goal_handle.publish_feedback(feedback)
+
+        self._arm.set_start_state_to_current_state()
+        self._arm.set_goal_state(pose_stamped_msg=pose, pose_link=self._pose_link)
+
+        feedback.state = "executing"
+        goal_handle.publish_feedback(feedback)
+
+        plan_params = self._make_plan_params(pipeline, planner_id, plan_time, velocity_scale)
+        ok, msg = self._plan_and_execute(plan_params, goal_handle)
+        return self._finish_action(goal_handle, result, ok, msg)
+
+    def _execute_joints(self, goal_handle):
+        req = goal_handle.request
+        feedback = MoveToJoints.Feedback()
+        result = MoveToJoints.Result()
+
+        velocity_scale = max(0.01, min(1.0, float(req.speed)))
+        joints = list(req.joints)
+
+        self.get_logger().info(
+            f"[joints] {[f'{v:.3f}' for v in joints]}  speed={velocity_scale:.2f}"
+        )
+
+        feedback.state = "planning"
+        goal_handle.publish_feedback(feedback)
+
+        # Формируем целевое состояние по суставным координатам
+        goal_state = RobotState(self._robot_model)
+        goal_state.set_joint_group_positions(self._planning_group, joints)
+        goal_state.update()
+
+        self._arm.set_start_state_to_current_state()
+        self._arm.set_goal_state(robot_state=goal_state)
+
+        feedback.state = "executing"
+        goal_handle.publish_feedback(feedback)
+
+        plan_params = self._make_plan_params(
+            "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
+        )
+        ok, msg = self._plan_and_execute(plan_params, goal_handle)
+        return self._finish_action(goal_handle, result, ok, msg)
+
+    def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):
+        name = request.name.strip()
+        velocity_scale = max(0.01, min(1.0, float(request.speed)))
+
+        self.get_logger().info(f"[named] name='{name}'  speed={velocity_scale:.2f}")
+
+        self._arm.set_start_state_to_current_state()
+        try:
+            self._arm.set_goal_state(configuration_name=name)
+        except Exception as exc:
+            response.success = False
+            response.message = f"Неизвестное состояние '{name}': {exc}"
+            self.get_logger().error(response.message)
+            return response
+
+        plan_params = self._make_plan_params(
+            "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
+        )
+        plan_result = self._arm.plan(single_plan_parameters=plan_params)
+        if not plan_result:
+            response.success = False
+            response.message = f"Не удалось построить траекторию для '{name}'"
+            self.get_logger().error(response.message)
+            return response
+
+        self._moveit.execute(plan_result.trajectory, controllers=[])
+        response.success = True
+        response.message = f"Переместился в '{name}'"
+        self.get_logger().info(response.message)
+        return response
+
+    def _handle_stop(self, request: Trigger.Request, response: Trigger.Response):
+        try:
+            self._moveit.get_trajectory_execution_manager().stop_execution()
+            response.success = True
+            response.message = "Выполнение траектории остановлено"
+        except Exception as exc:
+            response.success = False
+            response.message = str(exc)
+        self.get_logger().info(f"[stop] {response.message}")
+        return response
 
 
 def main(args=None):
-    rclpy.init(args=args)
-
-    # MoveItPy создаёт C++ узел с именем из лонча → читает robot_description_kinematics и т.д.
-    moveit = MoveItPy(node_name="move_to_pose_server")
-    arm: PlanningComponent = moveit.get_planning_component(PLANNING_GROUP)
-
-    # Отдельный лёгкий узел для сервиса — другое имя, нет конфликта параметров
-    node = Node("move_to_pose_service")
-    logger = node.get_logger()
-
-    cb_group = ReentrantCallbackGroup()
-
-    def handle(request: MoveToPose.Request, response: MoveToPose.Response):
-        pose = request.pose
-        if not pose.header.frame_id:
-            pose.header.frame_id = "base_link"
-
-        arm.set_start_state_to_current_state()
-        arm.set_goal_state(pose_stamped_msg=pose, pose_link=POSE_LINK)
-
-        logger.info(
-            f"Planning to ({pose.pose.position.x:.3f}, "
-            f"{pose.pose.position.y:.3f}, {pose.pose.position.z:.3f})"
-        )
-        plan_result = arm.plan()
-
-        if not plan_result:
-            response.success = False
-            response.message = "Planning failed: pose may be unreachable or in collision"
-            logger.error(response.message)
-            return response
-
-        moveit.execute(plan_result.trajectory, controllers=[])
-        response.success = True
-        response.message = "Motion executed successfully"
-        logger.info(response.message)
-        return response
-
-    node.create_service(MoveToPose, "iiwa/move_to_pose", handle, callback_group=cb_group)
-    logger.info(f"MoveToPoseServer ready (pose_link={POSE_LINK})")
-
     try:
+        rclpy.init(args=args)
+        node = IiwaMotionServer()
         executor = MultiThreadedExecutor()
         executor.add_node(node)
         executor.spin()
     except (KeyboardInterrupt, ExternalShutdownException):
         pass
     finally:
-        node.destroy_node()
         rclpy.shutdown()
 
 

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -54,6 +54,15 @@ class ControllerCfg:
     moveit: MoveitCfg
 
 
+@dataclass(frozen=True)
+class PlanningCfg:
+    pose_link: str          # TCP-линк для декартовых целей
+    planning_group: str     # Группа планирования из SRDF
+    default_frame: str      # Система отсчёта по умолчанию
+    default_planner: str    # Планировщик по умолчанию
+    planning_attempts: int  # Число попыток планирования
+
+
 @dataclass(frozen=True)
 class FoxgloveCfg:
     enabled: bool                       # Запускать ли foxglove_bridge
@@ -87,6 +96,7 @@ class Settings:
     robot: RobotCfg
     digital_twin: DigitalTwinCfg
     controller: ControllerCfg
+    planning: PlanningCfg
     foxglove: FoxgloveCfg
 
     def to_dict(self) -> Dict[str, Any]:
@@ -276,6 +286,16 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
         moveit=moveit,
     )
 
+    # planning
+    planning_raw = raw.get("planning", {})
+    planning = PlanningCfg(
+        pose_link=str(planning_raw.get("pose_link", "link_ee")),
+        planning_group=str(planning_raw.get("planning_group", "iiwa_arm")),
+        default_frame=str(planning_raw.get("default_frame", "base_link")),
+        default_planner=str(planning_raw.get("default_planner", "ompl")),
+        planning_attempts=int(planning_raw.get("planning_attempts", 3)),
+    )
+
     # foxglove
     foxglove = _parse_foxglove(raw.get("foxglove"))
 
@@ -283,6 +303,7 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
         robot=robot,
         digital_twin=digital_twin,
         controller=controller,
+        planning=planning,
         foxglove=foxglove,
     )