feat: enhance motion sequence capabilities and add configuration support

- Updated README.md to include examples for data collection and motion sequence testing.
- Modified setting.yaml to change the TCP link for planning from "patron" to "link_ee".
- Refactored move_to_pose_server.py to improve planner configuration readability.
- Updated setup.py to include motion_config.json in package resources and added a new console script for testing motion sequences.
- Introduced motion_config.json to define home joints and poses for motion sequences.
- Added test_motion_sequence.py to implement a test runner for executing motion sequences and recording data to a bag file.

README.md

@@ -47,7 +47,7 @@ LC_ALL=C ros2 launch iiwa_moveit demo...
 sudo pip3 install transforms3d --break-system-packages
 ```
 Отправка робота в точку:
-```
+```bash
 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'}"
 
@@ -63,6 +63,14 @@ ros2 service call /iiwa/move_to_named iiwa_msgs/srv/MoveToNamedPose \
 ros2 service call /iiwa/stop std_srvs/srv/Trigger "{}"
 ```
 
+Пример сбора данных:
+```bash
+ros2 run iiwa_utils test_motion_sequence --ros-args -p n_iterations:=5 -p bag_path:=test_bag/ -p topics:="['/joint_states', '/d455_top/color/image_raw/image', '/d455_top/image_raw/camera_info']"
+
+ros2 run iiwa_utils test_motion_sequence \
+  --ros-args -p config_path:=/path/to/my_config.json 
+```
+
 Спавн объекта:
 ```bash
 {

src/iiwa_config/config/setting.yaml

@@ -34,7 +34,7 @@ controller:
 
 
 planning:
-  pose_link: "patron"        # TCP-линк для декартовых целей
+  pose_link: "link_ee"        # TCP-линк для декартовых целей
   planning_group: "iiwa_arm"  # Группа планирования из SRDF
   default_frame: "base_link"  # Система отсчёта по умолчанию
   default_planner: "ompl"     # Планировщик по умолчанию

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -19,7 +19,7 @@ from iiwa_msgs.action import MoveToPose, MoveToJoints
 from iiwa_msgs.srv import MoveToNamedPose
 
 
-# Таблица планировщиков: имя → (pipeline, planner_id, время планирования в секундах)
+# Таблица планировщиков
 PLANNERS = {
     "ompl":  ("ompl", "RRTConnectkConfigDefault", 10.0),
     "ptp":   ("pilz_industrial_motion_planner", "PTP", 2.0),

src/iiwa_utils/iiwa_utils/motion_config.json

@@ -0,0 +1,913 @@
+{
+  "home_joints": [
+    0.0,
+    0.0,
+    0.0,
+    -1.57,
+    0.0,
+    1.57,
+    0.0
+  ],
+  "poses": [
+    {
+      "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"
+    },
+    {
+      "x": 0.58,
+      "y": -0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 2.8791,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.58,
+      "y": 0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 2.8791,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.56,
+      "y": 0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 2.9666,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.56,
+      "y": -0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 2.9666,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.54,
+      "y": -0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.0541,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.54,
+      "y": 0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.0541,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.52,
+      "y": 0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.1416,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.52,
+      "y": -0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.1416,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.5,
+      "y": -0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.2291,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.5,
+      "y": 0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.2291,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.48,
+      "y": 0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.3166,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.48,
+      "y": -0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.3166,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.46,
+      "y": -0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.4041,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.46,
+      "y": 0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.4041,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.44,
+      "y": 0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.4916,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.44,
+      "y": -0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 3.4916,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.44,
+      "y": -0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.4916,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.44,
+      "y": 0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.4916,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.46,
+      "y": 0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.4041,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.46,
+      "y": -0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.4041,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.48,
+      "y": -0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.3166,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.48,
+      "y": 0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.3166,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.5,
+      "y": 0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.2291,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.5,
+      "y": -0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.2291,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.52,
+      "y": -0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.1416,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.52,
+      "y": 0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.1416,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.54,
+      "y": 0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.0541,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.54,
+      "y": -0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 3.0541,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.56,
+      "y": -0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 2.9666,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.56,
+      "y": 0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 2.9666,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.58,
+      "y": 0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 2.8791,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.58,
+      "y": -0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 2.8791,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.6,
+      "y": -0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 2.7916,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.6,
+      "y": 0.1,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 2.7916,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.6,
+      "y": 0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 2.7916,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.6,
+      "y": -0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 2.7916,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.58,
+      "y": -0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 2.8791,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.58,
+      "y": 0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 2.8791,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.56,
+      "y": 0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 2.9666,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.56,
+      "y": -0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 2.9666,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.54,
+      "y": -0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.0541,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.54,
+      "y": 0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.0541,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.52,
+      "y": 0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.1416,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.52,
+      "y": -0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.1416,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.5,
+      "y": -0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.2291,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.5,
+      "y": 0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.2291,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.48,
+      "y": 0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.3166,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.48,
+      "y": -0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.3166,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.46,
+      "y": -0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.4041,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.46,
+      "y": 0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.4041,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.44,
+      "y": 0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.4916,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.44,
+      "y": -0.1,
+      "z": 0.45,
+      "a": 3.14,
+      "b": 0.4189,
+      "c": 3.4916,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.44,
+      "y": -0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.4916,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.44,
+      "y": 0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.4916,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.46,
+      "y": 0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.4041,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.46,
+      "y": -0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.4041,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.48,
+      "y": -0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.3166,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.48,
+      "y": 0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.3166,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.5,
+      "y": 0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.2291,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.5,
+      "y": -0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.2291,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.52,
+      "y": -0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.1416,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.52,
+      "y": 0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.1416,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.54,
+      "y": 0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.0541,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.54,
+      "y": -0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 3.0541,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.56,
+      "y": -0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 2.9666,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.56,
+      "y": 0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 2.9666,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.58,
+      "y": 0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 2.8791,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.58,
+      "y": -0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 2.8791,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.6,
+      "y": -0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 2.7916,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.6,
+      "y": 0.1,
+      "z": 0.4,
+      "a": 3.14,
+      "b": 0.4712,
+      "c": 2.7916,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.6,
+      "y": 0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 2.7916,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.6,
+      "y": -0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 2.7916,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.58,
+      "y": -0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 2.8791,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.58,
+      "y": 0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 2.8791,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.56,
+      "y": 0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 2.9666,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.56,
+      "y": -0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 2.9666,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.54,
+      "y": -0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.0541,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.54,
+      "y": 0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.0541,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.52,
+      "y": 0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.1416,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.52,
+      "y": -0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.1416,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.5,
+      "y": -0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.2291,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.5,
+      "y": 0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.2291,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.48,
+      "y": 0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.3166,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.48,
+      "y": -0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.3166,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.46,
+      "y": -0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.4041,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.46,
+      "y": 0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.4041,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "x": 0.44,
+      "y": 0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.4916,
+      "speed": 0.4,
+      "planner": "lin"
+    },
+    {
+      "x": 0.44,
+      "y": -0.1,
+      "z": 0.35,
+      "a": 3.14,
+      "b": 0.5236,
+      "c": 3.4916,
+      "speed": 0.3,
+      "planner": "lin"
+    }
+  ]
+}

src/iiwa_utils/iiwa_utils/test_motion_sequence.py

@@ -0,0 +1,237 @@
+#!/usr/bin/env python3
+import json
+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
+
+
+class IiwaTestRunner(Node):
+    def __init__(self):
+        super().__init__('iiwa_test_runner')
+
+        self.declare_parameter('n_iterations', 3)
+        self.declare_parameter('bag_path', '/tmp/iiwa_test')
+        self.declare_parameter('config_path', '')
+        self.declare_parameter('topics', [
+            '/joint_states',
+            '/iiwa/joint_states',
+            '/tf',
+            '/tf_static',
+        ])
+        self.declare_parameter("delay_between_iterations", 5.0)
+
+        self._n_iter = self.get_parameter('n_iterations').value
+        self._delay_between_iterations = self.get_parameter("delay_between_iterations").value
+        self._bag_path = self.get_parameter('bag_path').value
+        self._topics_param = self.get_parameter('topics').value
+        config_path = self.get_parameter('config_path').value
+
+        cfg = self._load_config(config_path)
+        self._home_joints: list[float] = cfg['home_joints']
+        self._poses: list[dict] = cfg['poses']
+
+        self._cb_group = ReentrantCallbackGroup()
+        self._joints_client = ActionClient(
+            self, MoveToJoints, '/iiwa/move_to_joints',
+            callback_group=self._cb_group,
+        )
+        self._pose_client = ActionClient(
+            self, MoveToPose, '/iiwa/move_to_pose',
+            callback_group=self._cb_group,
+        )
+
+        self._writer: rosbag2_py.SequentialWriter | None = None
+        self._registered_topics: set[str] = set()
+        self._subs = []
+
+        self._init_bag()
+        self._init_subscribers()
+
+    # Config
+    def _load_config(self, config_path: str) -> dict:
+        path = Path(config_path) if config_path else Path(__file__).parent / 'motion_config.json'
+        self.get_logger().info(f'Loading config from {path}')
+        with open(path) as f:
+            return json.load(f)
+
+    # Bag files
+    def _init_bag(self):
+        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())
+
+        for topic in self._topics_param:
+            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, joints: list[float], speed: float = 0.1) -> bool:
+        goal = MoveToJoints.Goal()
+        goal.joints = joints
+        goal.speed = speed
+
+        self.get_logger().info(f'[MOVE] joints → {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()
+        success_holder: list[bool] = [False]
+
+        def _on_result(future):
+            res = future.result().result
+            success_holder[0] = res.success
+            self.get_logger().info(f'[MOVE] joints done: success={res.success}')
+            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 success_holder[0]
+
+    def _send_pose_goal(self, *, x, y, z, a, b, c,
+                        speed: float = 0.1, 
+                        planner: str = 'lin',
+                        id: int = None) -> bool:
+        goal = MoveToPose.Goal()
+        goal.x, goal.y, goal.z = x, y, z
+        goal.a, goal.b, goal.c = a, b, c
+        goal.speed = speed
+        goal.planner = planner
+
+        self.get_logger().info(f'[MOVE] {id if id is not None else ""} pose → x={x} y={y} z={z} planner={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()
+        success_holder: list[bool] = [False]
+
+        def _on_result(future):
+            res = future.result().result
+            success_holder[0] = res.success
+            self.get_logger().info(f'[MOVE] pose done: success={res.success}')
+            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 success_holder[0]
+
+    # 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_joints_goal(self._home_joints)
+
+                for i, pose in enumerate(self._poses):
+                    self._send_pose_goal(id=i, **pose)
+
+                self._send_joints_goal(self._home_joints)
+                time.sleep(self._delay_between_iterations)
+
+            self.get_logger().info('======= Sequence complete =======')
+        finally:
+            self.close_bag()
+            done_event.set()
+
+
+def main():
+    rclpy.init()
+    node = IiwaTestRunner()
+
+    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()

src/iiwa_utils/setup.py

@@ -10,6 +10,7 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name, ['iiwa_utils/motion_config.json']),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -20,8 +21,8 @@ setup(
     entry_points={
         'console_scripts': [
             "object_spawner = iiwa_utils.object_spawner:main",
-            "motion_planning_test = iiwa_utils.motion_planing_test:main",
             "camera_spawner = iiwa_utils.camera_spawner:main",
+            "test_motion_sequence = iiwa_utils.test_motion_sequence:main",
         ],
     },
 )