diff --git a/README.md b/README.md index df3ac29..cd2034f 100644 --- a/README.md +++ b/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 { diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index 5c33e2e..7a16d71 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/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" # Планировщик по умолчанию diff --git a/src/iiwa_planning/scripts/move_to_pose_server.py b/src/iiwa_planning/scripts/move_to_pose_server.py index efb1cfc..9b42e42 100644 --- a/src/iiwa_planning/scripts/move_to_pose_server.py +++ b/src/iiwa_planning/scripts/move_to_pose_server.py @@ -19,13 +19,13 @@ 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), - "lin": ("pilz_industrial_motion_planner", "LIN", 2.0), - "circ": ("pilz_industrial_motion_planner", "CIRC", 2.0), - "chomp": ("chomp", "", 10.0), + "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), } diff --git a/src/iiwa_utils/iiwa_utils/motion_config.json b/src/iiwa_utils/iiwa_utils/motion_config.json new file mode 100644 index 0000000..5f96dd0 --- /dev/null +++ b/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" + } + ] +} diff --git a/src/iiwa_utils/iiwa_utils/test_motion_sequence.py b/src/iiwa_utils/iiwa_utils/test_motion_sequence.py new file mode 100644 index 0000000..4da1270 --- /dev/null +++ b/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() diff --git a/src/iiwa_utils/setup.py b/src/iiwa_utils/setup.py index b799e4b..658fc61 100644 --- a/src/iiwa_utils/setup.py +++ b/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", ], }, )