diff --git a/lucy_config_generator/lucy_config_generator/templates/gazebo.xacro.j2 b/lucy_config_generator/lucy_config_generator/templates/gazebo.xacro.j2 index a8c32d8..a547037 100644 --- a/lucy_config_generator/lucy_config_generator/templates/gazebo.xacro.j2 +++ b/lucy_config_generator/lucy_config_generator/templates/gazebo.xacro.j2 @@ -26,7 +26,7 @@ {% for sensor in sensors %} - + 10 {{ sensor.topic }} {% for s in sensor.sensors %} diff --git a/lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py b/lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py index 33d23c8..8713c58 100644 --- a/lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py +++ b/lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py @@ -6,6 +6,7 @@ from __future__ import annotations +from collections import deque from dataclasses import dataclass from dataclasses import field import os @@ -14,6 +15,7 @@ import signal import subprocess import tempfile +import threading import time from typing import List, Optional @@ -52,6 +54,7 @@ class ControlSupervisorNode(Node): GAZEBO_RUNNING_TOPIC = '/lucy/gazebo_running' TERMINATE_TIMEOUT_S = 10.0 SPAWN_SETTLE_S = 2.0 + CHILD_LOG_TAIL = 50 def __init__(self) -> None: super().__init__('lucy_control_supervisor') @@ -175,9 +178,43 @@ def _start_rsp(self, cfg: _StackConfig, urdf_xml: str) -> None: stderr=subprocess.STDOUT, env=os.environ.copy(), ) - self._children.append(_ManagedProc('robot_state_publisher', popen, 'rsp')) + self._track('robot_state_publisher', popen, 'rsp') time.sleep(self.SPAWN_SETTLE_S) + 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. + """ + child = _ManagedProc(name, popen, kind) + self._children.append(child) + tail: deque = deque(maxlen=self.CHILD_LOG_TAIL) + + def pump() -> None: + try: + for line in iter(popen.stdout.readline, b''): + text = line.decode('utf-8', 'replace').rstrip() + if text: + tail.append(text) + self.get_logger().debug(f'[{name}] {text}') + except (OSError, ValueError): + pass + # poll rather than wait: _terminate_children polls these same handles. + deadline = time.monotonic() + self.TERMINATE_TIMEOUT_S + code = popen.poll() + while code is None and time.monotonic() < deadline: + time.sleep(0.1) + code = popen.poll() + if code: + self.get_logger().error(f'{name} exited with code {code}') + for text in tail: + self.get_logger().error(f'[{name}] {text}') + + threading.Thread(target=pump, name=f'pump-{name}', daemon=True).start() + def _start_cm(self, cfg: _StackConfig) -> None: cmd = [ 'ros2', @@ -196,7 +233,7 @@ def _start_cm(self, cfg: _StackConfig) -> None: stderr=subprocess.STDOUT, env=os.environ.copy(), ) - self._children.append(_ManagedProc('ros2_control_node', popen, 'cm')) + self._track('ros2_control_node', popen, 'cm') time.sleep(self.SPAWN_SETTLE_S) def _start_spawners(self, cfg: _StackConfig) -> Optional[str]: @@ -221,7 +258,7 @@ def _start_spawners(self, cfg: _StackConfig) -> Optional[str]: stderr=subprocess.STDOUT, env=os.environ.copy(), ) - self._children.append(_ManagedProc(f'spawner_{name}', popen, 'spawner')) + self._track(f'spawner_{name}', popen, 'spawner') time.sleep(1.0) return None diff --git a/lucy_ros2_control/CMakeLists.txt b/lucy_ros2_control/CMakeLists.txt index d7552d7..4d98f9c 100644 --- a/lucy_ros2_control/CMakeLists.txt +++ b/lucy_ros2_control/CMakeLists.txt @@ -33,11 +33,39 @@ target_include_directories(lucy_ros2_control PUBLIC $ $ ) -ament_target_dependencies( - lucy_ros2_control PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} +# Link the namespaced CMake targets rather than ament_target_dependencies. +# ament_target_dependencies expands to each dependency's full ament link list, +# which includes its __rosidl_generator_py libraries. Those are built to be +# loaded inside a Python interpreter: they carry undefined _Py* symbols and +# deliberately do not link libpython. Dragging them into a C++ plugin makes +# macOS's flat-namespace dlopen fail to resolve them, so pluginlib rejects the +# hardware component with "symbol not found in flat namespace +# '_PyExc_RuntimeError'" and controller_manager never loads any hardware. +target_link_libraries(lucy_ros2_control PUBLIC + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) +if(APPLE) + # Those dependencies expand to each message package's __rosidl_generator_py + # library. Those are built to be loaded inside a Python interpreter: they carry + # undefined _Py* symbols and deliberately do not link libpython, because in + # `python3` the symbols are already in the process. + # + # This plugin uses no symbol from any of them, but they still land in its load + # commands. ros2_control_node is pure C++, so when pluginlib dlopen()s the + # plugin the flat-namespace lookup for '_PyExc_RuntimeError' finds nothing and + # the hardware component is rejected — controller_manager then loads no + # hardware, spawns no controllers, and nothing publishes /joint_states. + # + # -dead_strip_dylibs drops dylibs no symbol is used from, removing them before + # they can be loaded. Linux binds these lazily and never hits this, so keep the + # flag APPLE-only. + target_link_options(lucy_ros2_control PRIVATE "LINKER:-dead_strip_dylibs") +endif() + # Export hardware plugins pluginlib_export_plugin_description_file(hardware_interface lucy_ros2_control.xml)