Add a self-running demo and a one-step Windows start

.gitattributes

@@ -1,2 +1,3 @@
 *.py text eol=lf
 *.sh text eol=lf
+*.cmd text eol=crlf

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

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

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

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

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

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
     ])

waybionic_bringup/package.xml

@@ -29,6 +29,7 @@
   <exec_depend>waybionic_teleop</exec_depend>
   <exec_depend>waybionic_camera</exec_depend>
 
+  <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

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])