60 lines
3.6 KiB
Python
60 lines
3.6 KiB
Python
"""Manual local JTC integration check; never opens a FRI connection."""
|
|
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-controller-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')).toxml()
|
|
assert '<param name="simulate">false</param>' in xml
|
|
# Exercise our plugin's explicit local simulation path, with no FRI socket.
|
|
xml = xml.replace('<param name="simulate">false</param>', '<param name="simulate">true</param>')
|
|
params = work / 'robot.yaml'
|
|
params.write_text(yaml.safe_dump({'/**': {'ros__parameters': {'robot_description': xml}}}))
|
|
processes = []
|
|
handles = []
|
|
try:
|
|
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')]),
|
|
]:
|
|
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=env, 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
|
|
print('SMOKE PASS: actual URDF, iiwa hardware plugin, broadcaster and JTC active; no FRI connection.', flush=True)
|
|
finally:
|
|
for process in processes:
|
|
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)
|