diff --git a/.gitattributes b/.gitattributes
index 61d299b..cc4ed6d 100644
--- a/.gitattributes
+++ b/.gitattributes
@@ -1,2 +1,3 @@
*.py text eol=lf
*.sh text eol=lf
+*.cmd text eol=crlf
diff --git a/BuildInstructions.md b/BuildInstructions.md
index 289602e..5105335 100644
--- a/BuildInstructions.md
+++ b/BuildInstructions.md
@@ -445,6 +445,26 @@ window; with no controller input, teleop disables and holds the arm after the
start the bridge again and press Start. Real drives need a stop that does not
depend on the controller.
+### Self-Running Demo
+
+`autoplay:=true` plays a scripted demo as soon as the ground station is ready, and again
+whenever the controller has been left alone for 30 seconds. The demo lifts the arm, draws a square and a vertical line with the tool
+tip, and tilts the tool about its tip, showing the path and a caption in RViz.
+Touching any button, stick or trigger stops the demo at once and leaves teleop disabled;
+press Start to drive the arm yourself.
+Leave the controller alone for 30 seconds and the demo starts again. Autoplay runs
+only with simulated drives.
+
+**Windows, in one step:** double-click `scripts\windows-demo.cmd`. It does three things:
+
+- starts Docker Desktop if it isn't running;
+- opens the controller bridge in its own window, if Python 3 is installed;
+- starts the ground station.
+
+Press **Ctrl+C** in its window to stop.
+
+**Linux:** `ros2 launch waybionic_bringup ground_station.launch.py teleop:=true autoplay:=true`.
+
## Real MKS Drives over CAN
`drive_interface` sends the same frames to real MKS SERVO42D/57D drives through a
diff --git a/compose.yaml b/compose.yaml
index 2409d83..e15d0c3 100644
--- a/compose.yaml
+++ b/compose.yaml
@@ -175,3 +175,19 @@ services:
- teleop:=true
- joy_source:=udp
- joy_udp_bind:=0.0.0.0
+
+ # The teleop demo that plays itself whenever the controller is left idle.
+ wslg-demo:
+ <<: *wslg
+ profiles: [wslg-demo]
+ ports:
+ - 127.0.0.1:47300:47300/udp
+ command:
+ - ros2
+ - launch
+ - waybionic_bringup
+ - ground_station.launch.py
+ - teleop:=true
+ - joy_source:=udp
+ - joy_udp_bind:=0.0.0.0
+ - autoplay:=true
diff --git a/scripts/windows-demo.cmd b/scripts/windows-demo.cmd
new file mode 100644
index 0000000..df2c5bc
--- /dev/null
+++ b/scripts/windows-demo.cmd
@@ -0,0 +1,4 @@
+@echo off
+rem Starts the self-running teleop demo on Windows. Double-click this file, or run it from a terminal.
+powershell -NoProfile -ExecutionPolicy Bypass -File "%~dp0windows-demo.ps1"
+if errorlevel 1 pause
diff --git a/scripts/windows-demo.ps1 b/scripts/windows-demo.ps1
new file mode 100644
index 0000000..ef8c2c7
--- /dev/null
+++ b/scripts/windows-demo.ps1
@@ -0,0 +1,84 @@
+# Starts the self-running teleop demo on Windows in one step: Docker Desktop, the controller
+# bridge (if Python is installed) and the ground station with autoplay. Run windows-demo.cmd,
+# or: powershell -ExecutionPolicy Bypass -File scripts\windows-demo.ps1
+
+$root = Split-Path -Parent $PSScriptRoot
+$docker = Join-Path $env:LOCALAPPDATA 'Programs\DockerDesktop\resources\bin\docker.exe'
+if (-not (Test-Path $docker)) {
+ $command = Get-Command docker -ErrorAction SilentlyContinue
+ if (-not $command) {
+ Write-Error 'Docker Desktop is not installed; see BuildInstructions.md.'
+ exit 1
+ }
+ $docker = $command.Source
+}
+
+& $docker info *> $null
+if ($LASTEXITCODE -ne 0) {
+ $app = @(
+ (Join-Path $env:LOCALAPPDATA 'Programs\DockerDesktop\Docker Desktop.exe'),
+ (Join-Path $env:ProgramFiles 'Docker\Docker\Docker Desktop.exe')
+ ) | Where-Object { Test-Path $_ } | Select-Object -First 1
+ if (-not $app) {
+ Write-Error 'Docker is not running and Docker Desktop was not found.'
+ exit 1
+ }
+ Write-Host 'Starting Docker Desktop...'
+ Start-Process $app
+ $deadline = (Get-Date).AddMinutes(3)
+ do {
+ Start-Sleep -Seconds 3
+ & $docker info *> $null
+ } until ($LASTEXITCODE -eq 0 -or (Get-Date) -gt $deadline)
+ if ($LASTEXITCODE -ne 0) {
+ Write-Error 'Docker Desktop did not start within 3 minutes.'
+ exit 1
+ }
+}
+
+# The bridge needs Python 3. Without Python installed, Windows still has a python.exe
+# placeholder that fails, and python may be Python 2, so ask each one for version 3.
+$python = $null
+$pythonArguments = @()
+foreach ($name in 'py', 'python', 'python3') {
+ $command = Get-Command $name -ErrorAction SilentlyContinue
+ if (-not $command) {
+ continue
+ }
+ # Not an if expression: its output would unroll @('-3') into the string '-3'.
+ $prefix = @()
+ if ($command.Name -eq 'py.exe') {
+ $prefix = @('-3')
+ }
+ & $command.Source @prefix -c 'import sys; sys.exit(sys.version_info[0] != 3)' *> $null
+ if ($LASTEXITCODE -eq 0) {
+ $python, $pythonArguments = $command, $prefix
+ break
+ }
+}
+$bridge = $null
+if ($python) {
+ $arguments = $pythonArguments + @('-m', 'waybionic_teleop.xinput_bridge')
+ $bridge = Start-Process -FilePath $python.Source -ArgumentList $arguments -PassThru `
+ -WorkingDirectory (Join-Path $root 'waybionic_teleop')
+ if ($bridge.WaitForExit(2000)) {
+ Write-Warning ('The controller bridge stopped right after starting, so a controller ' +
+ 'cannot take over. To see why, run this in the waybionic_teleop folder: ' +
+ "$($python.Name) $($arguments -join ' ')")
+ }
+} else {
+ Write-Warning 'Python 3 was not found, so the demo runs but a controller cannot take over.'
+}
+
+Push-Location $root
+try {
+ Write-Host 'Starting the ground station; press Ctrl+C here to stop.'
+ & $docker compose run --rm --build --service-ports wslg-demo
+} finally {
+ Pop-Location
+ if ($bridge -and -not $bridge.HasExited) {
+ Stop-Process -Id $bridge.Id
+ }
+}
+# With -File, PowerShell exits with 0 unless told otherwise; windows-demo.cmd pauses on failure.
+exit $LASTEXITCODE
diff --git a/waybionic_bringup/CMakeLists.txt b/waybionic_bringup/CMakeLists.txt
index ada6c76..d95c5c8 100644
--- a/waybionic_bringup/CMakeLists.txt
+++ b/waybionic_bringup/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(ament_cmake REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
+ find_package(ament_cmake_pytest REQUIRED)
find_package(ament_cmake_ros REQUIRED)
find_package(launch_testing_ament_cmake REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
@@ -25,6 +26,8 @@ if(BUILD_TESTING)
add_launch_test(test/test_cartesian_launch.py ${isolated})
add_launch_test(test/test_joint_demo_launch.py ${isolated})
add_launch_test(test/test_teleop_launch.py ${isolated})
+ add_launch_test(test/test_autoplay_launch.py ${isolated})
+ ament_add_pytest_test(test_launch_checks test/test_launch_checks.py TIMEOUT 60)
endif()
install(PROGRAMS scripts/joint_demo.py scripts/camera_follower.py
diff --git a/waybionic_bringup/launch/ground_station.launch.py b/waybionic_bringup/launch/ground_station.launch.py
index b7d24fb..c152679 100644
--- a/waybionic_bringup/launch/ground_station.launch.py
+++ b/waybionic_bringup/launch/ground_station.launch.py
@@ -7,8 +7,8 @@
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import (
- AndSubstitution, Command, EqualsSubstitution, LaunchConfiguration, NotSubstitution,
- OrSubstitution)
+ AndSubstitution, Command, EqualsSubstitution, IfElseSubstitution, LaunchConfiguration,
+ NotSubstitution, OrSubstitution)
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
@@ -26,6 +26,11 @@ def check_files_exist(context, *args, **kwargs):
joy_source = LaunchConfiguration('joy_source').perform(context)
if joy_source not in ('device', 'udp', 'none'):
raise ValueError(f'joy_source must be device, udp or none, not {joy_source}')
+ if IfCondition(LaunchConfiguration('autoplay')).evaluate(context):
+ if not IfCondition(LaunchConfiguration('teleop')).evaluate(context):
+ raise RuntimeError('autoplay plays the controller demo, so it needs teleop:=true')
+ if LaunchConfiguration('drive_interface').perform(context) != 'sim':
+ raise RuntimeError('autoplay only runs with simulated drives')
return []
@@ -84,6 +89,11 @@ def generate_launch_description():
'joy_source', default_value='device',
description='Controller input: device (local joystick), udp (host bridge) or none')
+ autoplay_arg = DeclareLaunchArgument(
+ 'autoplay', default_value='false',
+ description='Play a scripted demo whenever the controller is left idle '
+ '(teleop with simulated drives only)')
+
joy_udp_bind_arg = DeclareLaunchArgument(
'joy_udp_bind', default_value='127.0.0.1',
description='Address the UDP controller bridge listens on (0.0.0.0 inside Docker)')
@@ -150,10 +160,14 @@ def generate_launch_description():
NotSubstitution(simulated_joints)))
)
+ # With autoplay, the controller goes through the autoplay node before it reaches teleop.
+ autoplay = AndSubstitution(teleop, LaunchConfiguration('autoplay'))
+ operator_joy = [('joy', IfElseSubstitution(autoplay, 'joy_operator', 'joy'))]
+
joy_node = Node(
package='joy', executable='game_controller_node', name='joy',
condition=IfCondition(AndSubstitution(teleop, EqualsSubstitution(joy_source, 'device'))),
- parameters=[sim_time]
+ parameters=[sim_time], remappings=operator_joy
)
joy_udp_node = Node(
@@ -163,7 +177,13 @@ def generate_launch_description():
{'bind_address': LaunchConfiguration('joy_udp_bind')},
{'port': ParameterValue(LaunchConfiguration('joy_udp_port'), value_type=int)},
diagnostics_topic, sim_time
- ]
+ ],
+ remappings=operator_joy
+ )
+
+ autoplay_node = Node(
+ package='waybionic_teleop', executable='autoplay', name='autoplay', output='screen',
+ condition=IfCondition(autoplay), parameters=[diagnostics_topic, sim_time]
)
teleop_node = Node(
@@ -245,10 +265,10 @@ def generate_launch_description():
return LaunchDescription([
model_arg, use_mock_diag_arg, diag_topic_arg, start_temp_pub_arg,
use_jsp_gui_arg, demo_mode_arg, demo_speed_arg, teleop_arg, drive_interface_arg,
- drive_channel_arg, joy_source_arg,
+ drive_channel_arg, joy_source_arg, autoplay_arg,
joy_udp_bind_arg, joy_udp_port_arg, follow_camera_arg, use_sim_time_arg, camera_arg,
camera_bind_arg, camera_log_arg,
launch_rviz_arg, rviz_config_arg, file_check, rsp_node, jsp_gui_node, joint_demo_node,
- joy_node, joy_udp_node, teleop_node, drives_node, camera_follower_node,
+ joy_node, joy_udp_node, autoplay_node, teleop_node, drives_node, camera_follower_node,
temp_diag_pub_node, camera_launch, rviz_node
])
diff --git a/waybionic_bringup/package.xml b/waybionic_bringup/package.xml
index 8e6d86e..ee3c14c 100644
--- a/waybionic_bringup/package.xml
+++ b/waybionic_bringup/package.xml
@@ -29,6 +29,7 @@
waybionic_teleop
waybionic_camera
+ ament_cmake_pytest
ament_cmake_ros
ament_lint_auto
ament_lint_common
diff --git a/waybionic_bringup/test/test_autoplay_launch.py b/waybionic_bringup/test/test_autoplay_launch.py
new file mode 100644
index 0000000..908d850
--- /dev/null
+++ b/waybionic_bringup/test/test_autoplay_launch.py
@@ -0,0 +1,119 @@
+"""With autoplay and no controller, the demo drives the arm; touching the controller stops it."""
+
+import os
+import time
+import unittest
+
+from ament_index_python.packages import get_package_share_directory
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
+import launch
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import launch_testing
+import launch_testing.actions
+import pytest
+import rclpy
+from sensor_msgs.msg import JointState, Joy
+
+from waybionic_teleop import gamepad
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+ launch_file = os.path.join(
+ get_package_share_directory('waybionic_bringup'), 'launch', 'ground_station.launch.py')
+ return launch.LaunchDescription([
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(launch_file),
+ launch_arguments={
+ 'launch_rviz': 'false',
+ 'teleop': 'true',
+ 'joy_source': 'none',
+ 'autoplay': 'true',
+ }.items()),
+ launch_testing.actions.ReadyToTest(),
+ ])
+
+
+class TestAutoplay(unittest.TestCase):
+
+ def test_demo_plays_and_gives_way_to_the_controller(self):
+ rclpy.init()
+ node = rclpy.create_node('autoplay_test')
+ joints, values, rows = {}, {}, {}
+ node.create_subscription(
+ JointState, '/joint_states',
+ lambda message: joints.update(zip(message.name, message.position)), 10)
+
+ def on_diagnostics(message):
+ for status in message.status:
+ rows[status.name] = status
+ if status.values:
+ values[status.name] = status.values[0].value
+ node.create_subscription(DiagnosticArray, '/diagnostics', on_diagnostics, 10)
+ operator = node.create_publisher(Joy, '/joy_operator', 10)
+
+ def spin_until(done, seconds):
+ deadline = time.monotonic() + seconds
+ while not done() and time.monotonic() < deadline:
+ rclpy.spin_once(node, timeout_sec=0.05)
+ return done()
+
+ def operate(seconds, *buttons, done=lambda: False, **axes):
+ """Hold the controller like this until done() or the time is up."""
+ message = Joy(axes=[axes.get(name, 0.0) for name in gamepad.AXES],
+ buttons=[int(name in buttons) for name in gamepad.BUTTONS])
+ deadline = time.monotonic() + seconds
+ while not done() and time.monotonic() < deadline:
+ operator.publish(message)
+ rclpy.spin_once(node, timeout_sec=0.02)
+ return done()
+
+ def disabled():
+ return values.get('teleop.state') == 'disabled'
+
+ def check_drives(gate):
+ """Check the drives' command gate, and that no drive faulted or refused a command."""
+ self.assertEqual(values.get('arm.command_gate'), gate,
+ rows.get('arm.command_gate'))
+ bus = {item.key: item.value for item in rows['can.bus'].values}
+ self.assertEqual(bus['rejected_commands'], '0')
+ drives = [status for name, status in rows.items() if name.startswith('drive.')]
+ self.assertEqual(len(drives), 6)
+ for status in drives:
+ self.assertEqual(status.level, DiagnosticStatus.OK,
+ f'{status.name}: {status.message}')
+
+ try:
+ # The demo enables teleop, lifts the arm in the upper group, then goes Cartesian.
+ self.assertTrue(spin_until(
+ lambda: values.get('teleop.group') == 'cartesian'
+ and values.get('teleop.state') == 'enabled', 45.0),
+ f'the demo never reached the Cartesian group: {values}')
+ self.assertGreater(joints['joint_2'], 0.5)
+ check_drives('enabled')
+
+ # Bumping a stick stops the demo and leaves teleop disabled, so the arm stays put.
+ self.assertTrue(operate(2.0, done=disabled, left_x=1.0))
+ pose = dict(joints)
+ operate(2.0, left_x=1.0)
+ self.assertTrue(disabled())
+ check_drives('stopped')
+ for joint in ('joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5'):
+ self.assertAlmostEqual(joints[joint], pose[joint], places=3)
+
+ # Start then hands the arm to the operator.
+ operate(0.2, 'start')
+ self.assertTrue(operate(2.0, done=lambda: values.get('teleop.state') == 'enabled'
+ and values.get('arm.command_gate') == 'enabled'))
+ check_drives('enabled')
+ finally:
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+
+ def test_exit_codes(self, proc_info):
+ launch_testing.asserts.assertExitCodes(proc_info, allowable_exit_codes=[0, -2])
diff --git a/waybionic_bringup/test/test_launch_checks.py b/waybionic_bringup/test/test_launch_checks.py
new file mode 100644
index 0000000..b1d475f
--- /dev/null
+++ b/waybionic_bringup/test/test_launch_checks.py
@@ -0,0 +1,39 @@
+"""The ground station launch refuses autoplay unless teleop runs on simulated drives."""
+
+import importlib.util
+from pathlib import Path
+
+from launch import LaunchContext
+import pytest
+
+LAUNCH_FILE = Path(__file__).resolve().parents[1] / 'launch' / 'ground_station.launch.py'
+SPEC = importlib.util.spec_from_file_location('ground_station_launch', LAUNCH_FILE)
+ground_station = importlib.util.module_from_spec(SPEC)
+SPEC.loader.exec_module(ground_station)
+
+
+def check(**arguments):
+ """Run the launch file's argument check; by default autoplay with teleop on sim drives."""
+ context = LaunchContext()
+ context.launch_configurations.update({
+ 'model': __file__, 'rvizconfig': __file__, 'demo_mode': 'false', 'teleop': 'true',
+ 'joy_source': 'udp', 'drive_interface': 'sim', 'autoplay': 'true', **arguments})
+ return ground_station.check_files_exist(context)
+
+
+def test_autoplay_runs_with_teleop_on_simulated_drives():
+ assert check() == []
+
+
+def test_real_drives_run_without_autoplay():
+ assert check(autoplay='false', drive_interface='socketcan') == []
+
+
+def test_autoplay_without_teleop_is_refused():
+ with pytest.raises(RuntimeError, match='needs teleop'):
+ check(teleop='false')
+
+
+def test_autoplay_with_real_drives_is_refused():
+ with pytest.raises(RuntimeError, match='only runs with simulated drives'):
+ check(drive_interface='socketcan')
diff --git a/waybionic_teleop/package.xml b/waybionic_teleop/package.xml
index 488bdae..237fc2f 100644
--- a/waybionic_teleop/package.xml
+++ b/waybionic_teleop/package.xml
@@ -8,6 +8,7 @@
Apache-2.0
diagnostic_msgs
+ geometry_msgs
rclpy
sensor_msgs
std_msgs
diff --git a/waybionic_teleop/setup.py b/waybionic_teleop/setup.py
index db0aed4..76495e0 100644
--- a/waybionic_teleop/setup.py
+++ b/waybionic_teleop/setup.py
@@ -29,6 +29,7 @@
'xbox_teleop = waybionic_teleop.xbox_teleop_node:main',
'sim_arm_drives = waybionic_teleop.sim_arm_drives_node:main',
'mks_drive_sim = waybionic_teleop.mks_drive_sim:main',
+ 'autoplay = waybionic_teleop.autoplay_node:main',
'joy_udp_receiver = waybionic_teleop.joy_udp_receiver:main',
],
},
diff --git a/waybionic_teleop/test/test_autoplay_node.py b/waybionic_teleop/test/test_autoplay_node.py
new file mode 100644
index 0000000..64c55f3
--- /dev/null
+++ b/waybionic_teleop/test/test_autoplay_node.py
@@ -0,0 +1,188 @@
+"""The autoplay node plays only on fresh reports from simulated drives, and hands over safely."""
+
+from pathlib import Path
+import time
+
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+import pytest
+import rclpy
+from sensor_msgs.msg import JointState, Joy
+from std_msgs.msg import String
+
+from waybionic_teleop import autoplay_node
+from waybionic_teleop.demo_script import SILENCE_S, STALE_S
+from waybionic_teleop.gamepad import AXES, BUTTON, BUTTONS
+
+URDF = (Path(__file__).resolve().parents[2] / 'waybionic_description' / 'urdf'
+ / 'waybionic_arm.urdf').read_text(encoding='utf-8')
+JOINTS = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5']
+DRIVES = ['base_yaw', 'shoulder', 'elbow', 'wrist_left', 'wrist_right', 'tool']
+RATE = 120.0
+# Teleop and the drives report twice a second.
+REPORT_TICKS = round(RATE / 2)
+TELEOP = DiagnosticArray(status=[
+ DiagnosticStatus(name='teleop.state', values=[KeyValue(key='value', value='enabled')]),
+ DiagnosticStatus(name='teleop.group', values=[KeyValue(key='value', value='base')])])
+
+
+def row(name, level, value, **extra):
+ return DiagnosticStatus(name=name, level=level, values=[
+ KeyValue(key=key, value=item) for key, item in {'value': value, **extra}.items()])
+
+
+def drives(interface, gate=DiagnosticStatus.WARN, drive=DiagnosticStatus.OK):
+ """Report like the drive node: its bus, command gate (waiting for Start) and each drive."""
+ return DiagnosticArray(status=[
+ row('can.bus', DiagnosticStatus.OK, '0', interface=interface),
+ row('arm.command_gate', gate, 'enabled' if gate == DiagnosticStatus.OK else 'stopped'),
+ *(row(f'drive.{name}', drive, '+0.000') for name in DRIVES)])
+
+
+def joy(*buttons, **axes):
+ return Joy(axes=[axes.get(name, 0.0) for name in AXES],
+ buttons=[int(name in buttons) for name in BUTTONS])
+
+
+def pressed(sent, button):
+ return [message for _, message in sent if message.buttons[BUTTON[button]]]
+
+
+class Clock:
+ """Stands in for the time module in the node, so the tests set the time."""
+
+ def __init__(self):
+ self.now = 100.0
+
+ def monotonic(self):
+ return self.now
+
+
+class Bench:
+ """The autoplay node on a set clock, with teleop and the drives reporting as asked."""
+
+ def __init__(self, node, clock):
+ self.node, self.clock, self.sent = node, clock, []
+ node.joy_publisher.publish = lambda message: self.sent.append((clock.now, message))
+ node.on_description(String(data=URDF))
+
+ def run(self, seconds, teleop=True, interface='sim', joints=True, operator=None,
+ **drive_levels):
+ """Tick for this long; return what reached teleop's joy topic, with when it was sent."""
+ start = len(self.sent)
+ for tick in range(round(seconds * RATE)):
+ if tick % REPORT_TICKS == 0:
+ if teleop:
+ self.node.on_diagnostics(TELEOP)
+ if interface:
+ self.node.on_diagnostics(drives(interface, **drive_levels))
+ if joints:
+ self.node.on_joint_states(JointState(name=JOINTS, position=[0.0] * len(JOINTS)))
+ if operator:
+ self.node.on_operator(operator)
+ self.node.tick()
+ self.clock.now += 1.0 / RATE
+ return self.sent[start:]
+
+
+@pytest.fixture
+def context(monkeypatch):
+ # Keep these nodes out of other tests' discovery.
+ monkeypatch.setenv('ROS_AUTOMATIC_DISCOVERY_RANGE', 'OFF')
+ context = rclpy.Context()
+ rclpy.init(context=context)
+ yield context
+ rclpy.try_shutdown(context=context)
+
+
+@pytest.fixture
+def bench(context, monkeypatch):
+ clock = Clock()
+ monkeypatch.setattr(autoplay_node, 'time', clock)
+ node = autoplay_node.Autoplay(context=context)
+ yield Bench(node, clock)
+ node.destroy_node()
+
+
+def test_the_demo_plays_on_simulated_drives(bench):
+ start = bench.clock.now
+ sent = bench.run(3.0)
+ # Silence first, so teleop times out however it was left, then Start.
+ assert sent[0][0] >= start + SILENCE_S - 1e-6
+ assert pressed(sent, 'start')
+
+
+def test_the_demo_never_plays_on_real_drives(bench):
+ assert bench.run(5.0, interface='socketcan') == []
+ # The operator's controller still reaches teleop, unchanged.
+ stick = joy(left_x=1.0)
+ bench.node.on_operator(stick)
+ assert bench.sent == [(bench.clock.now, stick)]
+
+
+@pytest.mark.parametrize('report', [
+ {'interface': None},
+ # A drive not set up or zeroed, or silent.
+ {'drive': DiagnosticStatus.WARN},
+ {'drive': DiagnosticStatus.ERROR},
+ # The drives have no valid robot description.
+ {'gate': DiagnosticStatus.ERROR},
+])
+def test_the_demo_waits_for_the_drives_to_be_ready(bench, report):
+ assert bench.run(5.0, **report) == []
+
+
+def test_start_waits_for_the_drives_gate_to_wait_for_it(bench):
+ # While the gate still passes commands, a Start would only disable teleop.
+ assert not pressed(bench.run(4.0, gate=DiagnosticStatus.OK), 'start')
+ assert pressed(bench.run(4.0), 'start')
+
+
+def test_the_demo_plays_on_only_once_the_drives_take_the_start(bench):
+ # Teleop reports enabled, but the drives' gate stays shut: the demo starts over.
+ sent = bench.run(6.0)
+ assert pressed(sent, 'start') and not pressed(sent, 'dpad_down')
+ assert pressed(bench.run(1.9), 'start')
+ # This time the gate opens, and the demo sets the speed.
+ assert pressed(bench.run(3.0, gate=DiagnosticStatus.OK), 'dpad_down')
+
+
+def test_the_demo_stops_while_another_node_publishes_joy(bench, context):
+ assert pressed(bench.run(3.0), 'start')
+ other = rclpy.create_node('joystick', context=context)
+ try:
+ other.create_publisher(Joy, 'joy', 10)
+ deadline = time.monotonic() + 5.0
+ while bench.node.count_publishers('/joy') < 2 and time.monotonic() < deadline:
+ time.sleep(0.01)
+ assert bench.run(5.0) == []
+ finally:
+ other.destroy_node()
+
+
+def test_taking_over_leaves_teleop_disabled(bench):
+ assert pressed(bench.run(3.0), 'start')
+ stick = joy(left_x=1.0)
+ sent = [message for _, message in bench.run(1.0, operator=stick)]
+ # Teleop is still enabled from the demo's Start: B first, then a neutral sample, and only
+ # then the operator's own input.
+ kinds = ['b' if message.buttons[BUTTON['b']] else 'operator' if message is stick
+ else 'neutral' for message in sent]
+ assert [kind for index, kind in enumerate(kinds)
+ if index == 0 or kind != kinds[index - 1]] == ['b', 'neutral', 'operator']
+ ours = [message for message in sent if message is not stick]
+ assert not any(any(message.axes) for message in ours)
+ assert {tuple(message.buttons) for message in ours} == {
+ tuple(joy('b').buttons), tuple(joy().buttons)}
+ assert not bench.node.playing
+
+
+@pytest.mark.parametrize('quiet', ['teleop', 'joints', 'interface'])
+def test_the_demo_sends_nothing_on_stale_reports_then_starts_over(bench, quiet):
+ assert pressed(bench.run(4.0), 'start')
+ silent_from = bench.clock.now
+ # Teleop times out once nothing more is sent.
+ assert all(at <= silent_from + STALE_S + 1e-6 for at, _ in bench.run(3.0, **{quiet: None}))
+ resumed = bench.clock.now
+ sent = bench.run(3.0)
+ assert sent[0][0] >= resumed + SILENCE_S - 1e-6
+ assert pressed(sent, 'start')
diff --git a/waybionic_teleop/test/test_can_drives.py b/waybionic_teleop/test/test_can_drives.py
index 51e1100..76a5ef1 100644
--- a/waybionic_teleop/test/test_can_drives.py
+++ b/waybionic_teleop/test/test_can_drives.py
@@ -485,3 +485,14 @@ def counted(reason):
# target, and start-up never logs a stale-command warning.
spin_until(executor, enabled_until(node), 1.0)
assert stops == [] and node.authorized and node.commanded == held
+
+
+@pytest.mark.parametrize('interface', ['sim', 'virtual'])
+def test_the_bus_report_names_the_interface(make_node, interface):
+ # The autoplay demo only plays when this says sim.
+ node, _ = make_node(interface=interface, channel=uuid.uuid4().hex)
+ reports = []
+ node.diagnostics_publisher.publish = reports.append
+ node.report()
+ bus = next(status for status in reports[0].status if status.name == 'can.bus')
+ assert {item.key: item.value for item in bus.values}['interface'] == interface
diff --git a/waybionic_teleop/test/test_demo_script.py b/waybionic_teleop/test/test_demo_script.py
new file mode 100644
index 0000000..68c9ade
--- /dev/null
+++ b/waybionic_teleop/test/test_demo_script.py
@@ -0,0 +1,105 @@
+"""The self-running demo steers teleop into the same Cartesian square from whatever it finds."""
+
+import math
+from pathlib import Path
+
+import pytest
+
+from waybionic_teleop.demo_script import (
+ CARTESIAN, DemoPlayer, LINES, operator_active, TeleopState)
+from waybionic_teleop.kinematics import ArmKinematics
+from waybionic_teleop.teleop import ArmTeleop, config_from_parameters
+
+URDF = (Path(__file__).resolve().parents[2] / 'waybionic_description' / 'urdf'
+ / 'waybionic_arm.urdf').read_text(encoding='utf-8')
+LIMITS = {'joint_1': (-math.pi, math.pi), 'joint_2': (-1.5708, 1.5708),
+ 'joint_3': (-1.5708, 1.5708), 'joint_4': (-1.5708, 1.5708),
+ 'joint_5': (-1.5708, 1.5708)}
+RATE = 120.0
+# The teleop node's input timeout and how often it reports its state.
+INPUT_TIMEOUT_S = 0.5
+REPORT_S = 0.5
+# The pause before the first edge, the edge itself, and the pause after it.
+FIRST_EDGE_TICKS = round(sum(seconds for seconds, *_ in CARTESIAN[:3]) * RATE)
+
+
+def play(teleop, kinematics, measured, seconds):
+ """Run the player against teleop and perfect drives; return the tip at each square start."""
+ player, state = DemoPlayer(RATE), TeleopState()
+ last, quiet, tips, caption = None, 0.0, [], None
+ for tick in range(round(seconds * RATE)):
+ if tick % round(REPORT_S * RATE) == 0:
+ state.enabled, state.group = teleop.enabled, teleop.active_group.name
+ # Perfect drives: ready, with the command gate open exactly while teleop is enabled.
+ state.drives_ready, state.gate_open = True, teleop.enabled
+ state.at_home = all(abs(measured[joint]) < 0.01 for joint in kinematics.joints)
+ sample = player.step(state)
+ # Like the teleop node: the last sample stays in force until the input times out.
+ quiet = quiet + 1.0 / RATE if sample is None else 0.0
+ last = sample or last
+ if quiet > INPUT_TIMEOUT_S:
+ if teleop.enabled:
+ teleop.disable(measured, 'Controller lost')
+ teleop.update((), (), measured, 1.0 / RATE)
+ elif last is not None:
+ teleop.update(*last, measured, 1.0 / RATE)
+ if teleop.targets:
+ measured = dict(teleop.targets)
+ if player.caption == LINES and caption != LINES:
+ tips.append((tick, kinematics.forward(measured)[0], teleop.active_group.name))
+ if tips and tick == tips[-1][0] + FIRST_EDGE_TICKS:
+ tips[-1] += (kinematics.forward(measured)[0], teleop.level)
+ caption = player.caption
+ return tips
+
+
+@pytest.fixture
+def make_teleop(parameters):
+ def make(**state):
+ kinematics = ArmKinematics.from_urdf(URDF)
+ teleop = ArmTeleop(config_from_parameters(parameters('xbox_teleop.yaml', 'xbox_teleop')),
+ LIMITS, kinematics)
+ for name, value in state.items():
+ setattr(teleop, name, value)
+ return teleop, kinematics
+ return make
+
+
+def test_the_demo_draws_the_same_square_from_a_fresh_start_and_on_every_loop(make_teleop):
+ teleop, kinematics = make_teleop()
+ measured = dict.fromkeys([*LIMITS, 'tool_grip'], 0.0)
+ squares = play(teleop, kinematics, measured, 90.0)
+ assert len(squares) >= 2
+ for _, start, group, end, level in squares[:2]:
+ assert (group, level) == ('cartesian', 2)
+ # The first edge pushes left at 25 mm/s for 1.7 s, so the tip moves about 41 mm.
+ assert 0.038 < end[1] - start[1] < 0.044
+ assert abs(end[0] - start[0]) < 5e-4 and abs(end[2] - start[2]) < 5e-4
+ assert squares[1][1] == pytest.approx(squares[0][1], abs=1e-3)
+
+
+def test_the_demo_recovers_whatever_state_the_operator_left(make_teleop):
+ fresh, kinematics = make_teleop()
+ reference = play(fresh, kinematics, dict.fromkeys([*LIMITS, 'tool_grip'], 0.0), 40.0)[0]
+ # Enabled in the Cartesian group at the slowest speed, with the arm away from home.
+ measured = {'joint_1': 0.6, 'joint_2': -0.4, 'joint_3': 0.9, 'joint_4': -0.7,
+ 'joint_5': 0.3, 'tool_grip': 0.5}
+ teleop, _ = make_teleop(level=0, group=2)
+ teleop.enable(measured, ())
+ assert teleop.enabled and teleop.active_group.name == 'cartesian'
+ _, start, group, end, level = play(teleop, kinematics, measured, 45.0)[0]
+ assert (group, level) == ('cartesian', 2)
+ assert start == pytest.approx(reference[1], abs=1e-3)
+ assert end == pytest.approx(reference[3], abs=1e-3)
+
+
+@pytest.mark.parametrize('axes, buttons, active', [
+ ([0.0] * 6, [0] * 21, False),
+ ([0.1, -0.1, 0.0, 0.0, 0.0, 0.0], [0] * 21, False),
+ ([0.0, 0.5, 0.0, 0.0, 0.0, 0.0], [0] * 21, True),
+ ([0.0, 0.0, 0.0, 0.0, 0.0, -0.6], [0] * 21, True),
+ ([0.0] * 6, [0] * 6 + [1] + [0] * 14, True),
+ ([math.nan] + [0.0] * 5, [0] * 21, False),
+])
+def test_operator_activity(axes, buttons, active):
+ assert operator_active(axes, buttons, 0.15) is active
diff --git a/waybionic_teleop/waybionic_teleop/autoplay_node.py b/waybionic_teleop/waybionic_teleop/autoplay_node.py
new file mode 100644
index 0000000..752818f
--- /dev/null
+++ b/waybionic_teleop/waybionic_teleop/autoplay_node.py
@@ -0,0 +1,216 @@
+"""
+Play the controller demo whenever nobody is using the controller.
+
+The real controller arrives on joy_operator and is passed on to teleop's joy topic. The demo
+takes over joy as soon as teleop is ready, and again after idle_s seconds without a button,
+stick or trigger moving; the moment someone touches the controller, it stops, presses B so
+teleop is left disabled, and passes the controller through. It only plays while the drives
+report that they are simulated and ready and no other node publishes joy, presses Start only
+while the drives' command gate waits for it, and sends nothing while teleop's state, the joint
+states or the drives' report are out of date, so teleop times out.
+The demo draws the tool tip's path and a caption in RViz.
+"""
+
+import math
+import time
+
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
+from geometry_msgs.msg import Point
+import rclpy
+from rclpy.executors import ExternalShutdownException
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile
+from sensor_msgs.msg import JointState, Joy
+from std_msgs.msg import String
+from visualization_msgs.msg import Marker, MarkerArray
+
+from waybionic_teleop.demo_script import (
+ DemoPlayer, operator_active, PIVOT, STALE_S, TeleopState)
+from waybionic_teleop.kinematics import ArmKinematics
+
+HOME_TOLERANCE_RAD = 0.01
+
+
+class Autoplay(Node):
+ """Switch teleop's controller input between the operator and the scripted demo."""
+
+ def __init__(self, **kwargs):
+ super().__init__('autoplay', **kwargs)
+ self.idle = float(self.declare_parameter('idle_s', 30.0).value)
+ self.deadzone = float(self.declare_parameter('deadzone', 0.15).value)
+ rate = float(self.declare_parameter('rate_hz', 120.0).value)
+ topic = self.declare_parameter('diagnostics_topic', '/diagnostics').value
+ self.player = DemoPlayer(rate)
+ self.state = TeleopState()
+ self.joints = {}
+ self.drive_interface, self.drives_reported = None, -math.inf
+ self.unready = []
+ self.kinematics = None
+ self.playing = False
+ self.handover = None
+ self.reason = None
+ # Nobody has used the controller yet, so the demo starts as soon as teleop is ready.
+ self.last_used = -math.inf
+ self.ticks = 0
+ self.caption, self.trail, self.fixed = None, [], None
+ latched = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
+ self.create_subscription(String, 'robot_description', self.on_description, latched)
+ self.create_subscription(Joy, 'joy_operator', self.on_operator, 10)
+ self.create_subscription(JointState, 'joint_states', self.on_joint_states, 10)
+ self.create_subscription(DiagnosticArray, topic, self.on_diagnostics, 10)
+ self.joy_publisher = self.create_publisher(Joy, 'joy', 10)
+ self.marker_publisher = self.create_publisher(MarkerArray, 'waybionic/teleop/markers', 10)
+ self.create_timer(1.0 / rate, self.tick)
+ self.get_logger().info(
+ f'Demo plays after {self.idle:.0f} s without controller input; touch it to stop')
+
+ def on_description(self, message):
+ try:
+ self.kinematics = ArmKinematics.from_urdf(message.data)
+ except ValueError as error:
+ self.get_logger().warning(f'No tool path in RViz: {error}')
+
+ def on_joint_states(self, message):
+ self.joints.update(zip(message.name, message.position))
+ self.state.measured = time.monotonic()
+
+ def on_diagnostics(self, message):
+ now = time.monotonic()
+ rows = {status.name: status for status in message.status}
+ for status in message.status:
+ values = {item.key: item.value for item in status.values}
+ if status.name == 'teleop.state':
+ self.state.enabled = values.get('value') == 'enabled'
+ self.state.reported = now
+ elif status.name == 'teleop.group':
+ self.state.group = values.get('value')
+ elif status.name == 'can.bus':
+ self.drive_interface, self.drives_reported = values.get('interface'), now
+ self.on_drives(rows)
+
+ def on_drives(self, rows):
+ """Read the command gate and drive rows the drive node reports with its bus."""
+ gate = rows.get('arm.command_gate')
+ drives = [status for name, status in rows.items() if name.startswith('drive.')]
+ self.unready = [status.name for status in drives if status.level != DiagnosticStatus.OK]
+ if gate is None or gate.level == DiagnosticStatus.ERROR:
+ self.unready.insert(0, 'arm.command_gate')
+ self.state.drives_ready = bool(drives) and not self.unready
+ self.state.gate_open = gate is not None and gate.level == DiagnosticStatus.OK
+
+ def on_operator(self, message):
+ if operator_active(message.axes, message.buttons, self.deadzone):
+ self.last_used = time.monotonic()
+ if self.playing:
+ self.playing = False
+ # The demo's Start left teleop enabled; B disables it before handing over.
+ self.handover = self.player.press('b')
+ self.draw()
+ self.get_logger().info('Controller in use: demo stopped; press Start to drive')
+ if not self.playing and self.handover is None:
+ self.joy_publisher.publish(message)
+
+ def blocked(self, now):
+ """Return why the demo cannot play now, or None when it can."""
+ if (self.kinematics is None or not self.state.fresh(now)
+ or now - self.drives_reported > STALE_S):
+ return 'waiting for teleop, the joint states and the drives to report'
+ if self.drive_interface != 'sim':
+ return f'the drives are not simulated ({self.drive_interface})'
+ if not self.state.drives_ready:
+ return 'the drives are not ready: ' + (', '.join(self.unready) or 'no drive rows')
+ if self.count_publishers(self.joy_publisher.topic_name) > 1:
+ return 'another node publishes joy'
+ return None
+
+ def tick(self):
+ now = time.monotonic()
+ if self.handover is not None:
+ sample = next(self.handover, None)
+ if sample is None:
+ self.handover = None
+ self.send(sample)
+ return
+ reason = self.blocked(now)
+ if reason != self.reason:
+ self.reason = reason
+ if reason:
+ self.get_logger().info(f'Demo on hold: {reason}')
+ # Send nothing, so teleop times out; the script starts over once all is well.
+ self.player.restart()
+ self.draw()
+ if reason:
+ return
+ if not self.playing:
+ if now - self.last_used < self.idle:
+ return
+ self.playing = True
+ self.player.restart()
+ self.get_logger().info('Controller idle: demo playing')
+ self.state.at_home = all(abs(self.joints.get(joint, math.inf)) < HOME_TOLERANCE_RAD
+ for joint in self.kinematics.joints)
+ self.send(self.player.step(self.state))
+ self.ticks += 1
+ if self.ticks % 4 == 0:
+ self.draw()
+
+ def send(self, sample):
+ if sample is None:
+ return
+ message = Joy(axes=sample[0], buttons=sample[1])
+ message.header.stamp = self.get_clock().now().to_msg()
+ self.joy_publisher.publish(message)
+
+ def draw(self):
+ caption = self.player.caption if self.playing else None
+ tip = None
+ if self.kinematics and all(joint in self.joints for joint in self.kinematics.joints):
+ tip = self.kinematics.forward(self.joints)[0]
+ if caption != self.caption:
+ # Each part of the demo starts a new path; the pivot also marks the fixed tip.
+ self.trail = []
+ self.fixed = tip if caption == PIVOT else None
+ self.caption = caption
+ if caption and tip is not None:
+ self.trail.append(tip)
+ items = [self.marker('demo_trail', Marker.LINE_STRIP, len(self.trail) > 1),
+ self.marker('demo_point', Marker.SPHERE, self.fixed is not None),
+ # RViz aborts on text with no visible glyphs, so an empty caption is deleted.
+ self.marker('demo_caption', Marker.TEXT_VIEW_FACING, bool(caption))]
+ trail, point, text = items
+ trail.points = [Point(x=x, y=y, z=z) for x, y, z in self.trail]
+ trail.scale.x = 0.004
+ trail.color.r, trail.color.g, trail.color.b, trail.color.a = 1.0, 0.45, 0.1, 1.0
+ if self.fixed is not None:
+ point.pose.position.x, point.pose.position.y, point.pose.position.z = self.fixed
+ point.scale.x = point.scale.y = point.scale.z = 0.024
+ point.color.r, point.color.g, point.color.b, point.color.a = 1.0, 0.15, 0.15, 0.6
+ text.text = caption or ''
+ text.pose.position.x, text.pose.position.y, text.pose.position.z = 0.42, -0.02, 0.36
+ text.scale.z = 0.05
+ text.color.r = text.color.g = text.color.b = text.color.a = 1.0
+ self.marker_publisher.publish(MarkerArray(markers=items))
+
+ @staticmethod
+ def marker(namespace, kind, shown):
+ item = Marker(ns=namespace, id=0, type=kind,
+ action=Marker.ADD if shown else Marker.DELETE)
+ item.header.frame_id = 'base_link'
+ item.pose.orientation.w = 1.0
+ return item
+
+
+def main():
+ rclpy.init()
+ node = Autoplay()
+ try:
+ rclpy.spin(node)
+ except (KeyboardInterrupt, ExternalShutdownException):
+ pass
+ finally:
+ node.destroy_node()
+ rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/waybionic_teleop/waybionic_teleop/demo_script.py b/waybionic_teleop/waybionic_teleop/demo_script.py
new file mode 100644
index 0000000..baf5fe8
--- /dev/null
+++ b/waybionic_teleop/waybionic_teleop/demo_script.py
@@ -0,0 +1,175 @@
+"""
+The self-running controller demo: what to press, and when, to show the arm off on its own.
+
+The player sends controller samples to teleop the way an operator would, and reads teleop's and
+the drives' state back from their diagnostics, so each run starts the same way whatever group,
+speed or pose the last operator left behind.
+"""
+
+from dataclasses import dataclass
+import math
+
+from waybionic_teleop.gamepad import AXES, BUTTONS
+
+# Longer than teleop's input timeout, so teleop disables itself however it was left.
+SILENCE_S = 1.0
+# Teleop and the drives report twice a second; anything older than this is out of date.
+STALE_S = 1.0
+# Edges of the square at 25 mm/s (50% speed): about 41 mm each.
+EDGE_S = 1.7
+LINES = 'STRAIGHT\nLINES'
+PIVOT = 'TIP\nFIXED'
+
+# Segments: seconds, caption, held buttons, stick axes. The upper group lifts the arm into a
+# working pose; the Cartesian group then draws a square and a vertical line and tilts the tool
+# about its tip.
+POSE = [
+ (2.0, None, (), {'left_x': 1.0, 'left_y': 1.0, 'right_y': 1.0}),
+ (0.7, None, (), {}),
+]
+CARTESIAN = [
+ (1.2, LINES, (), {}),
+ (EDGE_S, LINES, ('left_bumper',), {'left_x': 1.0}),
+ (0.6, LINES, (), {}),
+ (EDGE_S, LINES, ('left_bumper',), {'left_y': 1.0}),
+ (0.6, LINES, (), {}),
+ (EDGE_S, LINES, ('left_bumper',), {'left_x': -1.0}),
+ (0.6, LINES, (), {}),
+ (EDGE_S, LINES, ('left_bumper',), {'left_y': -1.0}),
+ (0.6, LINES, (), {}),
+ (1.6, LINES, ('left_bumper',), {'right_y': 1.0}),
+ (0.6, LINES, (), {}),
+ (1.6, LINES, ('left_bumper',), {'right_y': -1.0}),
+ (1.2, LINES, (), {}),
+ (1.0, PIVOT, (), {}),
+ (2.0, PIVOT, ('dpad_right',), {'right_x': 0.8}),
+ (3.0, PIVOT, ('dpad_left',), {'right_x': -0.8}),
+ (1.0, PIVOT, ('dpad_right',), {'right_x': 0.8}),
+ (1.2, PIVOT, (), {}),
+]
+
+
+@dataclass
+class TeleopState:
+ """What the player knows about teleop and the drives: from diagnostics and joint states."""
+
+ enabled: bool = None
+ group: str = None
+ at_home: bool = False
+ # Every drive ready with a valid model (arm.command_gate WARN or OK); the gate open (OK).
+ drives_ready: bool = False
+ gate_open: bool = False
+ # time.monotonic() of teleop's last report and of the last joint states.
+ reported: float = -math.inf
+ measured: float = -math.inf
+
+ def fresh(self, now):
+ """Return True while teleop's report and the joint states are both recent."""
+ return now - min(self.reported, self.measured) <= STALE_S
+
+
+def operator_active(axes, buttons, deadzone):
+ """Return True when someone is using the controller: a button held, a stick or trigger out."""
+ return any(buttons) or any(math.isfinite(value) and abs(value) > deadzone for value in axes)
+
+
+class DemoPlayer:
+ """Produce one controller sample per tick for the demo, over and over."""
+
+ def __init__(self, rate_hz):
+ self.rate = rate_hz
+ self.state = TeleopState()
+ self.caption = None
+ self.steps = None
+
+ def restart(self):
+ self.steps, self.caption = self.script(), None
+
+ def step(self, state):
+ """Return the next (axes, buttons) sample, or None to send nothing this tick."""
+ self.state = state
+ if self.steps is None:
+ self.restart()
+ for _ in range(2):
+ try:
+ return next(self.steps)
+ except StopIteration:
+ self.restart()
+ return None
+
+ def script(self):
+ yield from self.silence(SILENCE_S)
+ yield from self.hold(0.5)
+ # Like an operator, press Start only once the drives' gate is waiting for it.
+ if not (yield from self.wait(
+ lambda: self.state.drives_ready and not self.state.gate_open, 2.0)):
+ yield from self.hold(2.0)
+ return
+ yield from self.press('start')
+ if not (yield from self.wait(lambda: self.state.enabled and self.state.gate_open, 2.0)):
+ # Start was not taken (for example no joint states yet): try again shortly.
+ yield from self.hold(2.0)
+ return
+ # Three presses reach the slowest speed from any level, then two give 50%.
+ for button in ('dpad_down',) * 3 + ('dpad_up',) * 2:
+ yield from self.press(button)
+ yield from self.home()
+ if (yield from self.select('upper')):
+ yield from self.play(POSE)
+ if (yield from self.select('cartesian')):
+ yield from self.play(CARTESIAN)
+ yield from self.press('b')
+ yield from self.hold(2.0)
+
+ def sample(self, buttons=(), axes=None):
+ axes = axes or {}
+ return [axes.get(name, 0.0) for name in AXES], [int(name in buttons) for name in BUTTONS]
+
+ def ticks(self, seconds):
+ return max(1, round(seconds * self.rate))
+
+ def silence(self, seconds):
+ for _ in range(self.ticks(seconds)):
+ yield None
+
+ def hold(self, seconds, buttons=(), axes=None):
+ sample = self.sample(buttons, axes)
+ for _ in range(self.ticks(seconds)):
+ yield sample
+
+ def press(self, button):
+ yield from self.hold(0.2, (button,))
+ yield from self.hold(0.3)
+
+ def wait(self, done, seconds):
+ for _ in range(self.ticks(seconds)):
+ if done():
+ return True
+ yield self.sample()
+ return bool(done())
+
+ def home(self):
+ """Hold the home button until the arm is home, then a second longer to settle."""
+ sample = self.sample(('a',))
+ for _ in range(self.ticks(8.0)):
+ if self.state.at_home:
+ break
+ yield sample
+ yield from self.hold(1.0, ('a',))
+ yield from self.hold(0.3)
+
+ def select(self, group):
+ """Press the group button until teleop reports the group; False if it never does."""
+ for _ in range(4):
+ if self.state.group == group:
+ return True
+ yield from self.press('y')
+ # Teleop reports its group twice a second.
+ yield from self.hold(0.7)
+ return self.state.group == group
+
+ def play(self, segments):
+ for seconds, caption, buttons, axes in segments:
+ self.caption = caption
+ yield from self.hold(seconds, buttons, axes)
+ self.caption = None
diff --git a/waybionic_teleop/waybionic_teleop/sim_arm_drives_node.py b/waybionic_teleop/waybionic_teleop/sim_arm_drives_node.py
index 0fc3fa0..dc611aa 100644
--- a/waybionic_teleop/waybionic_teleop/sim_arm_drives_node.py
+++ b/waybionic_teleop/waybionic_teleop/sim_arm_drives_node.py
@@ -478,7 +478,8 @@ def report(self):
statuses = [status(
'can.bus', level, f'{frames:.0f}', 'frames/s',
f'{load:.0f}% worst-case load at {self.bitrate // 1000} kbit/s ({self.bus_label})',
- load_percent=f'{load:.1f}', errors=errors, rejected_commands=self.rejected)]
+ load_percent=f'{load:.1f}', errors=errors, rejected_commands=self.rejected,
+ interface=self.interface)]
gate_level = (DiagnosticStatus.OK if self.authorized else
DiagnosticStatus.ERROR if not self.description_valid else
DiagnosticStatus.WARN)