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)