diff --git a/lucy_bringup/launch/web_ros_api.launch.py b/lucy_bringup/launch/web_ros_api.launch.py index 3ea5401..8d3fe09 100644 --- a/lucy_bringup/launch/web_ros_api.launch.py +++ b/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( diff --git a/lucy_config_pipeline/src/pipeline/build.py b/lucy_config_pipeline/src/pipeline/build.py index 1e0ca89..7b26461 100644 --- a/lucy_config_pipeline/src/pipeline/build.py +++ b/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)}") diff --git a/lucy_config_pipeline/src/pipeline/flash.py b/lucy_config_pipeline/src/pipeline/flash.py index e4fd7e9..79bcafd 100644 --- a/lucy_config_pipeline/src/pipeline/flash.py +++ b/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)}") diff --git a/lucy_config_pipeline/src/services/config_services_node.py b/lucy_config_pipeline/src/services/config_services_node.py index 9ed4bb5..8e92c74 100644 --- a/lucy_config_pipeline/src/services/config_services_node.py +++ b/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: diff --git a/lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py b/lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py index 8713c58..0d6a8f4 100644 --- a/lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py +++ b/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,10 +162,11 @@ 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: @@ -143,12 +174,30 @@ def _terminate_children(self) -> None: 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) diff --git a/lucy_control_supervisor/test/test_node_argv.py b/lucy_control_supervisor/test/test_node_argv.py new file mode 100644 index 0000000..aafe334 --- /dev/null +++ b/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')