Update control move

2026-09-16 09:59:58 +10:00
parent d51c3737b9
commit 392036caf8
19 changed files with 603 additions and 130 deletions
@@ -0,0 +1,83 @@
+"""Manual bounded real-SDK/JTC integration check over loopback only.
+
+Run after building/sourcing iiwa_controller; never connects physical hardware.
+"""
+import csv
+import sys
+import time
+import os
+import signal
+import subprocess
+import tempfile
+from pathlib import Path
+import xacro
+import yaml
+
+root = Path(__file__).resolve().parents[3]
+work = Path(tempfile.mkdtemp(prefix='iiwa-fri-smoke-'))
+env = dict(os.environ, ROS_DOMAIN_ID='83', ROS_LOCALHOST_ONLY='1', ROS_LOG_DIR=str(work / 'logs'), ROS_HOME=str(work / 'ros'))
+env['CYCLONEDDS_URI'] = '<CycloneDDS><Domain><General><Interfaces><NetworkInterface name="lo"/></Interfaces><AllowMulticast>false</AllowMulticast></General><Discovery><Peers><Peer Address="127.0.0.1"/></Peers></Discovery></Domain></CycloneDDS>'
+xml = xacro.process_file(str(root / 'src/iiwa_description/urdf/iiwa7.urdf.xacro'), mappings={'robot_ip': '127.0.0.2', 'fri_port': '30283', 'fri_cycle_ms': '10'}).toxml()
+assert '<param name="simulate">false</param>' in xml
+assert '<param name="robot_ip">127.0.0.2</param>' in xml
+assert '<param name="fri_port">30283</param>' in xml
+build = Path(sys.argv[1]) if len(sys.argv) > 1 else root / 'build/iiwa_controller'
+server = build / 'fake_fri_server'
+shim = build / 'libfri_loopback_bind.so'
+assert server.is_file() and shim.is_file(), 'Build the test harness first'
+trace = work / 'commands.csv'
+params = work / 'robot.yaml'
+params.write_text(yaml.safe_dump({'/**': {'ros__parameters': {'robot_description': xml}}}))
+processes = []
+handles = []
+try:
+    server_log = (work / 'fake_fri_server.log').open('w')
+    handles.append(server_log)
+    processes.append(subprocess.Popen([str(server), str(trace)], stdout=server_log, stderr=subprocess.STDOUT, start_new_session=True))
+    time.sleep(0.1)
+    assert processes[0].poll() is None, 'Fake peer failed to bind'
+    for package, executable, extra in [
+        ('robot_state_publisher', 'robot_state_publisher', []),
+        ('controller_manager', 'ros2_control_node', ['--params-file', str(root / 'src/iiwa_config/config/moveit/iiwa_controller.yaml'), '-p', 'update_rate:=100', '-p', 'hardware_synchronization.expect_blocking_read_write:=true']),
+    ]:
+        log = (work / (executable + '.log')).open('w')
+        handles.append(log)
+        processes.append(subprocess.Popen(['ros2', 'run', package, executable, '--ros-args', '--params-file', str(params)] + extra,
+                                          env=dict(env, **({'LD_PRELOAD': str(shim)} if executable == 'ros2_control_node' else {})), stdout=log, stderr=subprocess.STDOUT, start_new_session=True))
+    for name in ['joint_state_broadcaster', 'iiwa_arm_controller']:
+        result = subprocess.run(['ros2', 'run', 'controller_manager', 'spawner', name, '--controller-manager-timeout', '15'],
+                                env=env, text=True, capture_output=True, timeout=25)
+        print(result.stdout, result.stderr, flush=True)
+        assert result.returncode == 0, name
+    result = subprocess.run(['ros2', 'control', 'list_controllers'], env=env, text=True, capture_output=True, timeout=10)
+    print(result.stdout, result.stderr, flush=True)
+    assert result.returncode == 0 and 'iiwa_arm_controller' in result.stdout and 'active' in result.stdout
+    initial = yaml.safe_load((root / 'src/iiwa_config/config/moveit/initial_positions.yaml').read_text())['initial_positions']
+    positions = [float(initial[f'joint{i}']) for i in range(1, 8)]
+    positions[0] += 0.02
+    goal = {'trajectory': {'joint_names': [f'joint{i}' for i in range(1, 8)],
+            'points': [{'positions': positions, 'time_from_start': {'sec': 2, 'nanosec': 0}}]}}
+    result = subprocess.run(['ros2', 'action', 'send_goal', '/iiwa_arm_controller/follow_joint_trajectory',
+                             'control_msgs/action/FollowJointTrajectory', yaml.safe_dump(goal)],
+                            env=env, text=True, capture_output=True, timeout=15)
+    print(result.stdout, result.stderr, flush=True)
+    assert result.returncode == 0 and 'SUCCEEDED' in result.stdout
+    rows = list(csv.DictReader(trace.open()))
+    assert len(rows) > 100 and processes[0].poll() is None, 'FRI stream stopped'
+    active = [r for r in rows if int(r['session']) == 4]
+    assert active and abs(float(active[-1]['position']) - 0.02) < 0.001
+    deltas = [abs(float(r['delta'])) for r in active]
+    intervals = [float(b['receive_seconds']) - float(a['receive_seconds']) for a, b in zip(active, active[1:])]
+    assert max(deltas) <= 1.71 * 0.01 + 1e-6, 'Command exceeds per-frame velocity limit'
+    print(f'SMOKE PASS: real SDK loopback + actual CM/JTC action succeeded; {len(active)} active frames, max delta={max(deltas):.8f} rad; receive interval min/max={min(intervals):.6f}/{max(intervals):.6f} s.', flush=True)
+finally:
+    for process in processes:
+        if process.poll() is None:
+            os.killpg(process.pid, signal.SIGINT)
+    for process in processes:
+        try: process.wait(timeout=5)
+        except subprocess.TimeoutExpired: os.killpg(process.pid, signal.SIGKILL)
+    for handle in handles: handle.close()
+    for log in work.glob('*.log'):
+        print('\n' + log.name + '\n' + log.read_text()[-4500:])
+    print('Logs:', work)