You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
111 lines
3.8 KiB
111 lines
3.8 KiB
"""Launch full teleop with webcam (no image server, with hand drivers).
|
|
|
|
Replicates start_teleop.sh but skips the image server that kills the UVC driver.
|
|
"""
|
|
import paramiko
|
|
import sys
|
|
import time
|
|
|
|
sys.stdout.reconfigure(encoding='utf-8', errors='replace')
|
|
|
|
ssh = paramiko.SSHClient()
|
|
ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy())
|
|
ssh.connect('10.0.0.64', username='unitree', password='123',
|
|
timeout=15, look_for_keys=False, allow_agent=False)
|
|
|
|
# ── Step 0: Kill any existing teleop / hand driver processes ──
|
|
print("Cleaning up old processes...")
|
|
_, o, _ = ssh.exec_command(
|
|
'ps aux | grep -E "teleop_hand_and_arm|ModbusDataHandler|image_server" | grep -v grep',
|
|
timeout=5)
|
|
procs = o.read().decode().strip()
|
|
if procs:
|
|
print(f" Found:\n{procs}")
|
|
for line in procs.split('\n'):
|
|
parts = line.split()
|
|
if len(parts) > 1:
|
|
ssh.exec_command(f'kill {parts[1]}', timeout=5)
|
|
time.sleep(1)
|
|
print(" Killed stale processes")
|
|
else:
|
|
print(" No stale processes")
|
|
|
|
# ── Step 1: Start Inspire hand Modbus-to-DDS drivers ──
|
|
print("\nStarting Inspire hand drivers...")
|
|
|
|
TELEOP_DIR = "/home/unitree/xr_teleoperate/teleop"
|
|
CYCLONE_HOME = "/home/unitree/g1-control/repos/cyclonedds/install/cyclonedds"
|
|
CYCLONE_URI = "file:///home/unitree/g1-control/repos/cyclonedds/cyclonedds.xml"
|
|
PYTHON = "/home/unitree/miniforge3/envs/tv/bin/python"
|
|
|
|
env_prefix = (
|
|
f'export CYCLONEDDS_HOME="{CYCLONE_HOME}" && '
|
|
f'export CYCLONEDDS_URI="{CYCLONE_URI}" && '
|
|
)
|
|
|
|
# Right hand (also initializes DDS)
|
|
right_hand_cmd = (
|
|
f'{env_prefix} '
|
|
f'cd {TELEOP_DIR} && '
|
|
f'nohup {PYTHON} -m inspire_hand.ModbusDataHandler '
|
|
f'--ip 192.168.123.211 --initDDS True > /tmp/right_hand.log 2>&1 &'
|
|
)
|
|
ssh.exec_command(right_hand_cmd, timeout=10)
|
|
print(" Right hand driver started (192.168.123.211)")
|
|
time.sleep(2)
|
|
|
|
# Left hand (check reachability first)
|
|
_, o, _ = ssh.exec_command('ping -c 1 -W 1 192.168.123.210 2>&1 | grep "1 received"', timeout=5)
|
|
left_reachable = bool(o.read().decode().strip())
|
|
if left_reachable:
|
|
left_hand_cmd = (
|
|
f'{env_prefix} '
|
|
f'cd {TELEOP_DIR} && '
|
|
f'nohup {PYTHON} -m inspire_hand.ModbusDataHandler '
|
|
f'--ip 192.168.123.210 > /tmp/left_hand.log 2>&1 &'
|
|
)
|
|
ssh.exec_command(left_hand_cmd, timeout=10)
|
|
print(" Left hand driver started (192.168.123.210)")
|
|
else:
|
|
print(" Left hand not reachable (skipped)")
|
|
|
|
time.sleep(2)
|
|
|
|
# ── Step 2: Launch teleop with webcam ──
|
|
print("\nStarting teleop with webcam...")
|
|
print("=" * 60)
|
|
|
|
teleop_cmd = (
|
|
f'{env_prefix} '
|
|
f'cd {TELEOP_DIR} && '
|
|
f'{PYTHON} teleop_hand_and_arm.py '
|
|
f'--arm=G1_29 --ee=inspire_ftp --webcam 0 --webcam-res 720p'
|
|
)
|
|
|
|
stdin, stdout, stderr = ssh.exec_command(teleop_cmd, timeout=300, get_pty=True)
|
|
|
|
# Stream output in real-time
|
|
start_time = time.time()
|
|
while time.time() - start_time < 120:
|
|
if stdout.channel.recv_ready():
|
|
data = stdout.channel.recv(4096).decode('utf-8', errors='replace')
|
|
print(data, end='')
|
|
if stderr.channel.recv_stderr_ready():
|
|
data = stderr.channel.recv_stderr(4096).decode('utf-8', errors='replace')
|
|
print(f"[err] {data}", end='')
|
|
if stdout.channel.exit_status_ready():
|
|
remaining = stdout.channel.recv(65536).decode('utf-8', errors='replace')
|
|
if remaining:
|
|
print(remaining, end='')
|
|
remaining_err = stderr.channel.recv_stderr(65536).decode('utf-8', errors='replace')
|
|
if remaining_err:
|
|
print(f"[err] {remaining_err}", end='')
|
|
exit_code = stdout.channel.recv_exit_status()
|
|
print(f"\n\nProcess exited with code {exit_code}")
|
|
break
|
|
time.sleep(0.1)
|
|
else:
|
|
print("\n\nTeleop is running! (2 min output window elapsed)")
|
|
print("Connect Quest 3 to: https://10.0.0.64:8012")
|
|
|
|
ssh.close()
|