Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@

{% for sensor in sensors %}
<gazebo>
<plugin filename="libmock_sensor.so" name="mock_sensor::MockSensorSystem">
<plugin filename="mock_sensor" name="mock_sensor::MockSensorSystem">
<update_rate>10</update_rate>
<topic_name>{{ sensor.topic }}</topic_name>
{% for s in sensor.sensors %}
Expand Down
43 changes: 40 additions & 3 deletions lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

from __future__ import annotations

from collections import deque
from dataclasses import dataclass
from dataclasses import field
import os
Expand All @@ -14,6 +15,7 @@
import signal
import subprocess
import tempfile
import threading
import time
from typing import List, Optional

Expand Down Expand Up @@ -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')
Expand Down Expand Up @@ -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',
Expand All @@ -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]:
Expand All @@ -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

Expand Down
34 changes: 31 additions & 3 deletions lucy_ros2_control/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -33,11 +33,39 @@ target_include_directories(lucy_ros2_control PUBLIC
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/src/include>
$<INSTALL_INTERFACE:include/lucy_ros2_control>
)
ament_target_dependencies(
lucy_ros2_control PUBLIC
${THIS_PACKAGE_INCLUDE_DEPENDS}
# Link the namespaced CMake targets rather than ament_target_dependencies.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Comment too long

# ament_target_dependencies expands to each dependency's full ament link list,
# which includes its <msg>__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

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same

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

Expand Down
Loading