276 lines
9.4 KiB
Python
276 lines
9.4 KiB
Python
#!/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')
|
|
|
|
|
|
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)
|
|
|
|
|
|
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}')
|
|
|
|
|
|
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)
|
|
|
|
|
|
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()
|