Refactor motion sequence handling: remove old test script, update setup, and implement new runner with configuration support

- Deleted obsolete `test_motion_sequence.py` file.
- Updated `setup.py` to remove references to the old test script and motion configuration file.
- Added `motion_sequence_config.json` to define home position and waypoints for the robot.
- Introduced `motion_sequence_runner.py` to execute motion sequences based on the new configuration format, supporting both joint and pose movements.

src/iiwa_planning/CMakeLists.txt

@@ -43,5 +43,16 @@ install(PROGRAMS
   RENAME move_to_pose_server
 )
 
+install(PROGRAMS
+  scripts/motion_sequence_runner.py
+  DESTINATION lib/${PROJECT_NAME}
+  RENAME motion_sequence_runner
+)
+
+install(FILES
+  scripts/motion_sequence_config.json
+  DESTINATION share/${PROJECT_NAME}
+)
+
 
 ament_package()

src/iiwa_planning/scripts/motion_sequence_config.json

@@ -0,0 +1,42 @@
+{
+  "home": {
+    "joints": [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0],
+    "speed": 0.1
+  },
+  "waypoints": [
+    {
+      "x": 0.6,
+      "y": 0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 2.7916,
+      "speed": 0.4,
+      "planner": "ptp"
+    },
+    {
+      "x": 0.6,
+      "y": -0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 2.7916,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "joints": [0.5, 0.3, 0.0, -1.2, 0.0, 1.4, 0.0],
+      "speed": 0.2
+    },
+    {
+      "x": 0.58,
+      "y": 0.0,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 2.8791,
+      "speed": 0.3,
+      "planner": "lin"
+    }
+  ]
+}

src/iiwa_planning/scripts/motion_sequence_runner.py

@@ -0,0 +1,279 @@
+#!/usr/bin/env python3
+"""
+Запуск заданной последовательности движений манипулятора iiwa.
+
+Каждая точка в конфиге может быть либо суставной (type: joints),
+либо декартовой (type: pose). Тип определяется автоматически по наличию
+ключа "joints" или координат "x/y/z".
+
+Запуск:
+  ros2 run iiwa_planning motion_sequence_runner \
+    --ros-args -p config_path:=/path/to/config.json -p n_iterations:=2
+"""
+
+import json
+import shutil
+import threading
+import time
+from pathlib import Path
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from rclpy.serialization import serialize_message
+
+import rosbag2_py
+from rosidl_runtime_py.utilities import get_message
+
+from iiwa_msgs.action import MoveToJoints, MoveToPose
+
+
+def _is_joints_waypoint(wp: dict) -> bool:
+    return "joints" in wp
+
+
+class MotionSequenceRunner(Node):
+    def __init__(self):
+        super().__init__('motion_sequence_runner')
+
+        self.declare_parameter('n_iterations', 3)
+        self.declare_parameter('bag_path', '')
+        self.declare_parameter('config_path', '')
+        self.declare_parameter('topics', [''])
+        self.declare_parameter('delay_between_iterations', 5.0)
+        self.declare_parameter('joints_action', 'cobot/move_to_joints')
+        self.declare_parameter('pose_action', 'cobot/move_to_pose')
+
+        self._n_iter = self.get_parameter('n_iterations').value
+        self._delay = self.get_parameter('delay_between_iterations').value
+        self._bag_path = self.get_parameter('bag_path').value
+        topics_param = self.get_parameter('topics').value
+        self._topics_param: list[str] = [t for t in topics_param if t]
+        config_path = self.get_parameter('config_path').value
+        joints_action = self.get_parameter('joints_action').value
+        pose_action = self.get_parameter('pose_action').value
+
+        cfg = self._load_config(config_path)
+        self._home: dict = cfg['home']
+        self._waypoints: list[dict] = cfg['waypoints']
+
+        self._cb_group = ReentrantCallbackGroup()
+        self._joints_client = ActionClient(
+            self, MoveToJoints, joints_action,
+            callback_group=self._cb_group,
+        )
+        self._pose_client = ActionClient(
+            self, MoveToPose, pose_action,
+            callback_group=self._cb_group,
+        )
+        self.get_logger().info(f'joints_action={joints_action}  pose_action={pose_action}')
+
+        self._writer: rosbag2_py.SequentialWriter | None = None
+        self._registered_topics: set[str] = set()
+        self._subs = []
+
+        if self._bag_path:
+            self._init_bag()
+            self._init_subscribers()
+        else:
+            self.get_logger().info('bag_path not set — recording disabled')
+
+    # ── Config ──────────────────────────────────────────────────────────────
+
+    def _load_config(self, config_path: str) -> dict:
+        path = (
+            Path(config_path) if config_path
+            else Path(__file__).parent / 'motion_sequence_config.json'
+        )
+        self.get_logger().info(f'Loading config from {path}')
+        with open(path) as f:
+            return json.load(f)
+
+    # ── Bag recording ────────────────────────────────────────────────────────
+
+    def _init_bag(self):
+        bag_dir = Path(self._bag_path)
+        if bag_dir.exists():
+            shutil.rmtree(bag_dir)
+            self.get_logger().info(f'Removed existing bag at {self._bag_path}')
+
+        storage_opts = rosbag2_py.StorageOptions(uri=self._bag_path, storage_id='mcap')
+        converter_opts = rosbag2_py.ConverterOptions(
+            input_serialization_format='cdr',
+            output_serialization_format='cdr',
+        )
+        self._writer = rosbag2_py.SequentialWriter()
+        self._writer.open(storage_opts, converter_opts)
+        self.get_logger().info(f'Bag opened at {self._bag_path}')
+
+    def _init_subscribers(self):
+        self.get_logger().info('Discovering topics (2 s)...')
+        time.sleep(2.0)
+        available = dict(self.get_topic_names_and_types())
+
+        topics = self._topics_param if self._topics_param else list(available.keys())
+        if not self._topics_param:
+            self.get_logger().info(
+                f'topics not set — recording all {len(topics)} available topics'
+            )
+
+        for topic in topics:
+            if topic not in available:
+                self.get_logger().warn(f'Topic {topic} not available, skipping')
+                continue
+
+            type_str = available[topic][0]
+            try:
+                msg_type = get_message(type_str)
+            except Exception as exc:
+                self.get_logger().warn(f'Cannot load type {type_str} for {topic}: {exc}')
+                continue
+
+            self._writer.create_topic(rosbag2_py.TopicMetadata(
+                id=len(self._registered_topics),
+                name=topic,
+                type=type_str,
+                serialization_format='cdr',
+            ))
+            self._registered_topics.add(topic)
+
+            def _make_cb(t: str):
+                def cb(msg):
+                    if self._writer:
+                        self._writer.write(
+                            t,
+                            serialize_message(msg),
+                            self.get_clock().now().nanoseconds,
+                        )
+                return cb
+
+            self._subs.append(self.create_subscription(
+                msg_type, topic, _make_cb(topic), 10,
+                callback_group=self._cb_group,
+            ))
+            self.get_logger().info(f'  subscribed: {topic} [{type_str}]')
+
+    def close_bag(self):
+        if self._writer:
+            del self._writer
+            self._writer = None
+            self.get_logger().info(f'Bag closed → {self._bag_path}')
+
+    # ── Action helpers ────────────────────────────────────────────────────────
+
+    def _send_joints_goal(self, wp: dict) -> bool:
+        goal = MoveToJoints.Goal()
+        goal.joints = wp['joints']
+        goal.speed = float(wp.get('speed', 0.1))
+
+        self.get_logger().info(f'[JOINTS] → {goal.joints}')
+        if not self._joints_client.wait_for_server(timeout_sec=10.0):
+            self.get_logger().error('MoveToJoints server unavailable')
+            return False
+
+        done = threading.Event()
+        result_holder: list[bool] = [False]
+
+        def _on_result(future):
+            result_holder[0] = future.result().result.success
+            self.get_logger().info(f'[JOINTS] done: success={result_holder[0]}')
+            done.set()
+
+        def _on_goal(future):
+            gh = future.result()
+            if not gh.accepted:
+                self.get_logger().error('MoveToJoints goal rejected')
+                done.set()
+                return
+            gh.get_result_async().add_done_callback(_on_result)
+
+        self._joints_client.send_goal_async(goal).add_done_callback(_on_goal)
+        done.wait()
+        return result_holder[0]
+
+    def _send_pose_goal(self, wp: dict, idx: int | None = None) -> bool:
+        goal = MoveToPose.Goal()
+        goal.x, goal.y, goal.z = wp['x'], wp['y'], wp['z']
+        goal.a, goal.b, goal.c = wp['a'], wp['b'], wp['c']
+        goal.speed = float(wp.get('speed', 0.1))
+        goal.planner = wp.get('planner', 'lin')
+
+        label = f'#{idx} ' if idx is not None else ''
+        self.get_logger().info(
+            f'[POSE] {label}→ x={goal.x} y={goal.y} z={goal.z} planner={goal.planner}'
+        )
+        if not self._pose_client.wait_for_server(timeout_sec=10.0):
+            self.get_logger().error('MoveToPose server unavailable')
+            return False
+
+        done = threading.Event()
+        result_holder: list[bool] = [False]
+
+        def _on_result(future):
+            result_holder[0] = future.result().result.success
+            self.get_logger().info(f'[POSE] done: success={result_holder[0]}')
+            done.set()
+
+        def _on_goal(future):
+            gh = future.result()
+            if not gh.accepted:
+                self.get_logger().error('MoveToPose goal rejected')
+                done.set()
+                return
+            gh.get_result_async().add_done_callback(_on_result)
+
+        self._pose_client.send_goal_async(goal).add_done_callback(_on_goal)
+        done.wait()
+        return result_holder[0]
+
+    def _send_waypoint(self, wp: dict, idx: int | None = None) -> bool:
+        if _is_joints_waypoint(wp):
+            return self._send_joints_goal(wp)
+        return self._send_pose_goal(wp, idx=idx)
+
+    # ── Main sequence ─────────────────────────────────────────────────────────
+
+    def run(self, done_event: threading.Event):
+        try:
+            for i in range(self._n_iter):
+                self.get_logger().info(f'======= Iteration {i + 1}/{self._n_iter} =======')
+
+                self._send_waypoint(self._home)
+
+                for idx, wp in enumerate(self._waypoints):
+                    self._send_waypoint(wp, idx=idx)
+
+                self._send_waypoint(self._home)
+                time.sleep(self._delay)
+
+            self.get_logger().info('======= Sequence complete =======')
+        finally:
+            self.close_bag()
+            done_event.set()
+
+
+def main():
+    rclpy.init()
+    node = MotionSequenceRunner()
+
+    executor = MultiThreadedExecutor()
+    executor.add_node(node)
+
+    done_event = threading.Event()
+    threading.Thread(target=node.run, args=(done_event,), daemon=True).start()
+
+    try:
+        while not done_event.is_set():
+            executor.spin_once(timeout_sec=0.1)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.close_bag()
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()