Kill supervisor orphaned children

lucy_bringup/launch/web_ros_api.launch.py

@@ -23,8 +23,8 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.actions import ExecuteProcess
 from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import AnyLaunchDescriptionSource
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch.substitutions import PathJoinSubstitution
@@ -49,18 +49,26 @@ def generate_launch_description():
         ),
     )
 
-    rosbridge = ExecuteProcess(
-        cmd=[
-            'ros2',
-            'launch',
-            'rosbridge_server',
-            'rosbridge_websocket_launch.xml',
-            'default_call_service_timeout:=5.0',
-            'call_services_in_new_thread:=true',
-            'send_action_goals_in_new_thread:=true',
+    # Included rather than run through `ros2 launch`: that shelled out via
+    # cmd.exe and a console-script shim, and on Windows shutdown signalled only
+    # the outermost of those, leaving rosbridge holding the port.
+    rosbridge = IncludeLaunchDescription(
+        AnyLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare('rosbridge_server'),
+                        'launch',
+                        'rosbridge_websocket_launch.xml',
+                    ]
+                )
+            ]
+        ),
+        launch_arguments=[
+            ('default_call_service_timeout', '5.0'),
+            ('call_services_in_new_thread', 'true'),
+            ('send_action_goals_in_new_thread', 'true'),
         ],
-        output='screen',
-        shell=True,
     )
 
     config_pipeline_launch = IncludeLaunchDescription(

lucy_config_pipeline/src/pipeline/build.py

@@ -3,6 +3,7 @@
 from collections.abc import Callable
 from pathlib import Path
 import subprocess
+import threading
 
 from .selection import board_build_plan
 from .selection import resolve_firmware_paths
@@ -106,19 +107,25 @@ def _run_command(
         bufsize=1,
     )
     assert process.stdout is not None
+    # The timeout has to arm before the read loop: that loop runs until the pipe
+    # closes, so a child that hangs without exiting never reaches process.wait().
+    # Killing closes the pipe, which ends the loop.
+    watchdog = threading.Timer(timeout_seconds, process.kill)
+    watchdog.start()
     emitted = 0
-    for line in process.stdout:
-        text = line.strip()
-        if not text:
-            continue
-        if emitted < 200:
-            feedback(phase=phase, progress=stream_progress, detail=text, board=board)
-        emitted += 1
     try:
-        return_code = process.wait(timeout=timeout_seconds)
-    except subprocess.TimeoutExpired:
-        process.kill()
-        process.wait(timeout=5)
+        for line in process.stdout:
+            text = line.strip()
+            if not text:
+                continue
+            if emitted < 200:
+                feedback(phase=phase, progress=stream_progress, detail=text, board=board)
+            emitted += 1
+        return_code = process.wait()
+    finally:
+        timed_out = not watchdog.is_alive()
+        watchdog.cancel()
+    if timed_out:
         raise TimeoutError(f"command timed out after {timeout_seconds}s: {' '.join(cmd)}")
     if return_code != 0:
         raise RuntimeError(f"command failed ({return_code}): {' '.join(cmd)}")

lucy_config_pipeline/src/pipeline/flash.py

@@ -4,6 +4,7 @@
 import os
 from pathlib import Path
 import subprocess
+import threading
 import time
 from typing import TYPE_CHECKING
 
@@ -217,19 +218,25 @@ def _run_command(
         bufsize=1,
     )
     assert process.stdout is not None
+    # The timeout has to arm before the read loop: that loop runs until the pipe
+    # closes, so a child that hangs without exiting never reaches process.wait().
+    # Killing closes the pipe, which ends the loop.
+    watchdog = threading.Timer(timeout_seconds, process.kill)
+    watchdog.start()
     emitted = 0
-    for line in process.stdout:
-        text = line.strip()
-        if not text:
-            continue
-        if emitted < 200:
-            feedback(phase=phase, progress=stream_progress, detail=text, board=board)
-        emitted += 1
     try:
-        return_code = process.wait(timeout=timeout_seconds)
-    except subprocess.TimeoutExpired:
-        process.kill()
-        process.wait(timeout=5)
+        for line in process.stdout:
+            text = line.strip()
+            if not text:
+                continue
+            if emitted < 200:
+                feedback(phase=phase, progress=stream_progress, detail=text, board=board)
+            emitted += 1
+        return_code = process.wait()
+    finally:
+        timed_out = not watchdog.is_alive()
+        watchdog.cancel()
+    if timed_out:
         raise TimeoutError(f"command timed out after {timeout_seconds}s: {' '.join(cmd)}")
     if return_code != 0:
         raise RuntimeError(f"command failed ({return_code}): {' '.join(cmd)}")

lucy_config_pipeline/src/services/config_services_node.py

@@ -160,7 +160,9 @@ def _on_get_mesh(self, req: GetMesh.Request, res: GetMesh.Response) -> GetMesh.R
                 res.success = False
                 res.message = f'mesh not found: {candidate}'
                 return res
-            res.data = candidate.read_text()
+            # COLLADA is UTF-8; without this the locale codec is used, which is
+            # cp1252 on a French Windows and mangles or rejects the file.
+            res.data = candidate.read_text(encoding='utf-8')
             res.success = True
             res.message = 'ok'
         except Exception as e:

lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py

@@ -12,8 +12,8 @@
 import os
 from pathlib import Path
 import shutil
-import signal
 import subprocess
+import sys
 import tempfile
 import threading
 import time
@@ -31,11 +31,37 @@
 from lucy_control_supervisor.controllers_spawn import controllers_to_spawn
 
 
+_WINDOWS_EXEC_SUFFIXES = ('.exe', '.bat', '.cmd')
+
+
+def node_argv(package: str, executable: str) -> list[str]:
+    """Absolute argv for a node, bypassing the ``ros2 run`` wrapper.
+
+    Signalling that wrapper leaves the node running, so a restart would stack a
+    second controller_manager on the orphaned first and both drive the joints.
+    """
+    from ament_index_python.packages import get_package_prefix
+
+    lib_dir = Path(get_package_prefix(package)) / 'lib' / package
+    matches = sorted(
+        (p for p in lib_dir.glob('*') if p.is_file() and p.stem == executable),
+        # Prefer something the OS can exec over a script needing an interpreter.
+        key=lambda p: 0 if not p.suffix or p.suffix.lower() in _WINDOWS_EXEC_SUFFIXES else 1,
+    )
+    if not matches:
+        raise RuntimeError(f'{package}/{executable} not found in {lib_dir}')
+    best = matches[0]
+    if os.name == 'nt' and best.suffix.lower() not in _WINDOWS_EXEC_SUFFIXES:
+        return [sys.executable, str(best)]
+    return [str(best)]
+
+
 @dataclass
 class _ManagedProc:
     name: str
     popen: subprocess.Popen
     kind: str  # rsp | cm | spawner
+    stopping: bool = False  # set when we ask it to stop, so the exit is expected
 
 
 @dataclass
@@ -55,6 +81,8 @@ class ControlSupervisorNode(Node):
     TERMINATE_TIMEOUT_S = 10.0
     SPAWN_SETTLE_S = 2.0
     CHILD_LOG_TAIL = 50
+    DISCOVERY_SETTLE_S = 2.0
+    CONTROLLER_MANAGER_NODE = 'controller_manager'
 
     def __init__(self) -> None:
         super().__init__('lucy_control_supervisor')
@@ -117,11 +145,13 @@ def _xacro_cmd(self, cfg: _StackConfig) -> list[str]:
             f"use_mock_hardware:={'true' if cfg.use_mock_hardware else 'false'}",
             f'ros2_control_file:={cfg.ros2_control_file}',
         ]
-        if shutil.which('ros2'):
-            return ['ros2', 'run', 'xacro', 'xacro', *tail]
+        try:
+            return [*node_argv('xacro', 'xacro'), *tail]
+        except (RuntimeError, KeyError):  # KeyError: package not in the index
+            pass
         if shutil.which('xacro'):
             return ['xacro', *tail]
-        raise RuntimeError('xacro not found on PATH')
+        raise RuntimeError('xacro not found')
 
     def _expand_robot_description(self, cfg: _StackConfig) -> str:
         cmd = self._xacro_cmd(cfg)
@@ -132,23 +162,42 @@ def _expand_robot_description(self, cfg: _StackConfig) -> str:
 
     def _terminate_children(self) -> None:
         for child in reversed(self._children):
+            child.stopping = True
             if child.popen.poll() is None:
                 try:
-                    child.popen.send_signal(signal.SIGTERM)
-                except ProcessLookupError:
+                    child.popen.terminate()
+                except OSError:
                     pass
         deadline = time.monotonic() + self.TERMINATE_TIMEOUT_S
         for child in self._children:
             while child.popen.poll() is None and time.monotonic() < deadline:
                 time.sleep(0.1)
         for child in self._children:
             if child.popen.poll() is None:
+                self.get_logger().warning(f'{child.name} ignored terminate; killing')
                 try:
                     child.popen.kill()
-                except ProcessLookupError:
+                except OSError:
                     pass
         self._children.clear()
 
+    def _foreign_controller_managers(self) -> List[str]:
+        """controller_manager nodes running that this supervisor did not start.
+
+        Two of them command the same joints, so the robot follows whichever
+        wrote last. Usually a stack left over from an earlier run.
+        """
+        if any(c.kind == 'cm' and c.popen.poll() is None for c in self._children):
+            return []
+        # Nothing of ours is running, so give discovery a moment to report
+        # anyone else's before concluding the graph is clear.
+        time.sleep(self.DISCOVERY_SETTLE_S)
+        return [
+            f"{namespace.rstrip('/')}/{name}"
+            for name, namespace in self.get_node_names_and_namespaces()
+            if name == self.CONTROLLER_MANAGER_NODE
+        ]
+
     def _start_rsp(self, cfg: _StackConfig, urdf_xml: str) -> None:
         payload = {
             'robot_state_publisher': {
@@ -164,10 +213,7 @@ def _start_rsp(self, cfg: _StackConfig, urdf_xml: str) -> None:
             yaml.safe_dump(payload, f, sort_keys=False)
             params_file = f.name
         cmd = [
-            'ros2',
-            'run',
-            'robot_state_publisher',
-            'robot_state_publisher',
+            *node_argv('robot_state_publisher', 'robot_state_publisher'),
             '--ros-args',
             '--params-file',
             params_file,
@@ -184,10 +230,8 @@ def _start_rsp(self, cfg: _StackConfig, urdf_xml: str) -> None:
     def _track(self, name: str, popen: subprocess.Popen, kind: str) -> None:
         """Register a child and keep draining its output.
 
-        These pipes had no reader, so a child blocked once the buffer filled and
-        a child that died left no trace. Children log through ROS already, so the
-        drained lines only go to debug; the tail is replayed at error level if the
-        child exits badly, which is when it is actually needed.
+        Unread pipes stall a chatty child once the buffer fills. Lines go to
+        debug; the tail is replayed at error level only if the child dies badly.
         """
         child = _ManagedProc(name, popen, kind)
         self._children.append(child)
@@ -208,7 +252,7 @@ def pump() -> None:
             while code is None and time.monotonic() < deadline:
                 time.sleep(0.1)
                 code = popen.poll()
-            if code:
+            if code and not child.stopping:
                 self.get_logger().error(f'{name} exited with code {code}')
                 for text in tail:
                     self.get_logger().error(f'[{name}] {text}')
@@ -217,10 +261,7 @@ def pump() -> None:
 
     def _start_cm(self, cfg: _StackConfig) -> None:
         cmd = [
-            'ros2',
-            'run',
-            'controller_manager',
-            'ros2_control_node',
+            *node_argv('controller_manager', 'ros2_control_node'),
             '--ros-args',
             '--params-file',
             str(cfg.controllers_yaml),
@@ -242,10 +283,7 @@ def _start_spawners(self, cfg: _StackConfig) -> Optional[str]:
             return 'no controllers found in controllers.yaml'
         for name in names:
             cmd = [
-                'ros2',
-                'run',
-                'controller_manager',
-                'spawner',
+                *node_argv('controller_manager', 'spawner'),
                 name,
                 '--switch-timeout',
                 '10',
@@ -272,6 +310,16 @@ def _restart_stack(self) -> tuple[bool, str]:
                 if not p.exists():
                     return False, f'missing path: {p}'
 
+            if not cfg.use_gazebo_sim and not cfg.gazebo_only:
+                foreign = self._foreign_controller_managers()
+                if foreign:
+                    return False, (
+                        f"controller_manager already running ({', '.join(foreign)}) "
+                        'and not owned by this supervisor. Two of them drive the '
+                        'same joints, so refusing to start a second. Stop the '
+                        'other Lucy stack, then retry.'
+                    )
+
             self._terminate_children()
             urdf_xml = self._expand_robot_description(cfg)
 

lucy_control_supervisor/test/test_node_argv.py

@@ -0,0 +1,37 @@
+"""node_argv must resolve nodes directly.
+
+Going through `ros2 run` makes the node a grandchild, so terminating what we
+hold a handle to leaves the node running and the next start stacks a second
+controller_manager on it.
+"""
+
+from pathlib import Path
+
+import pytest
+
+from lucy_control_supervisor.supervisor_node import node_argv
+
+NODES = [
+    ('controller_manager', 'ros2_control_node'),
+    ('controller_manager', 'spawner'),
+    ('robot_state_publisher', 'robot_state_publisher'),
+]
+
+
+@pytest.mark.parametrize('package,executable', NODES)
+def test_resolves_to_an_existing_file(package, executable):
+    argv = node_argv(package, executable)
+    assert Path(argv[-1]).is_file()
+    assert Path(argv[-1]).stem == executable
+
+
+@pytest.mark.parametrize('package,executable', NODES)
+def test_never_delegates_to_the_ros2_wrapper(package, executable):
+    argv = node_argv(package, executable)
+    assert 'run' not in argv
+    assert Path(argv[0]).stem != 'ros2'
+
+
+def test_unknown_executable_is_reported():
+    with pytest.raises(RuntimeError, match='not found'):
+        node_argv('controller_manager', 'no_such_node')