From 6c445e08a539ca11927e3bd1830aa2b49f7119c2 Mon Sep 17 00:00:00 2001 From: root Date: Tue, 23 Jun 2026 05:57:33 +0000 Subject: [PATCH 1/8] Add runtime dependency inference from controller manager Signed-off-by: root --- foreman/config/scenario.yaml | 11 -- .../adapters/component_state_monitor.py | 94 +++++++++++ foreman/foreman/engine.py | 6 + foreman/foreman/parser.py | 58 +------ foreman/foreman/planner.py | 4 + foreman/test/test_component_state_monitor.py | 146 ++++++++++++++++++ foreman/test/test_engine.py | 47 ++++++ foreman/test/test_parser.py | 39 +---- foreman/test/test_planner.py | 15 ++ 9 files changed, 317 insertions(+), 103 deletions(-) create mode 100644 foreman/test/test_component_state_monitor.py diff --git a/foreman/config/scenario.yaml b/foreman/config/scenario.yaml index 8348785..4de4667 100644 --- a/foreman/config/scenario.yaml +++ b/foreman/config/scenario.yaml @@ -5,17 +5,6 @@ hardware: lifecycle_nodes: - dummy_lifecycle_node -controllers: - joint_state_broadcaster: - requires: - - [kassow, inactive] - - [FrankaHardwareInterface, inactive] - - [dummy_lifecycle_node, active] - kassow_joint_trajectory_controller: - requires: [kassow, active] - franka_joint_trajectory_controller: - requires: [FrankaHardwareInterface, active] - goal_states: idle: diff --git a/foreman/foreman/adapters/component_state_monitor.py b/foreman/foreman/adapters/component_state_monitor.py index 1b8d332..980dc27 100644 --- a/foreman/foreman/adapters/component_state_monitor.py +++ b/foreman/foreman/adapters/component_state_monitor.py @@ -1,6 +1,8 @@ from typing import Dict, List from controller_manager_msgs.msg import ControllerManagerActivity +from controller_manager_msgs.srv import ListControllers +from controller_manager_msgs.srv import ListHardwareComponents from lifecycle_msgs.msg import TransitionEvent from lifecycle_msgs.srv import GetState from rclpy.event_handler import QoSSubscriptionMatchedInfo @@ -14,6 +16,8 @@ from foreman.engine import ForemanEngine from foreman.types import Component from foreman.types import ComponentType +from foreman.types import ControllerDependencyRule +from foreman.types import HardwareRequirement from foreman.types import LifecycleState @@ -64,6 +68,23 @@ def __init__( f"{self._logger_prefix} Subscribed to /{controller_manager_name}/activity" ) + # Service clients used to read the CM's controllers and hardware once, to infer dependencies. + self._client_list_controllers = self._node.create_client( + ListControllers, + f'/{controller_manager_name}/list_controllers', + callback_group=self._node.callback_group_services + ) + self._client_list_hardware_components = self._node.create_client( + ListHardwareComponents, + f'/{controller_manager_name}/list_hardware_components', + callback_group=self._node.callback_group_services + ) + # The two responses arrive separately, here None means "not received yet". + self._controller_states = None + self._hardware_components = None + # Inference runs only once, the first time the CM becomes visible. + self._dependencies_inferred = False + # --- Lifecycle node monitoring --- self._lc_node_get_state_clients: Dict[str, object] = {} @@ -96,6 +117,77 @@ def __init__( f"{self._logger_prefix} Monitoring lifecycle nodes: {lifecycle_nodes}" ) + @staticmethod + def infer_dependency_rules(controller_states, hardware_components): + """Map each controller to the hardware that owns the interfaces it requires.""" + interface_owner = {} + for hardware in hardware_components: + for interface in list(hardware.command_interfaces) + list(hardware.state_interfaces): + interface_owner[interface.name] = hardware.name + + dependency_rules = [] + for controller in controller_states: + required_interfaces = ( + set(controller.required_command_interfaces) + | set(controller.required_state_interfaces) + ) + # Resolve each interface to the hardware that owns it. + required_hardware_names = { + interface_owner[name] for name in required_interfaces if name in interface_owner + } + dependency_rules.append(ControllerDependencyRule( + controller_name=controller.name, + required_hardware=[ + HardwareRequirement(name=hardware_name, state=LifecycleState.ACTIVE) + for hardware_name in required_hardware_names + ], + )) + return dependency_rules + + def infer_dependencies(self): + """Ask the controller_manager (once) for its controllers and hardware to infer dependencies.""" + if self._dependencies_inferred: + return + if not (self._client_list_controllers.service_is_ready() + and self._client_list_hardware_components.service_is_ready()): + return + self._dependencies_inferred = True + + controllers_future = self._client_list_controllers.call_async(ListControllers.Request()) + controllers_future.add_done_callback(self._on_list_controllers_response) + + hardware_future = self._client_list_hardware_components.call_async( + ListHardwareComponents.Request()) + hardware_future.add_done_callback(self._on_list_hardware_components_response) + + def _on_list_controllers_response(self, future): + # Cache the controllers; rules are built once the hardware response also arrives. + try: + self._controller_states = future.result().controller + self._build_and_push_dependency_rules() + except Exception as e: + self._node.get_logger().warning( + f"{self._logger_prefix} list_controllers failed: {e}") + + def _on_list_hardware_components_response(self, future): + # Cache the hardware; rules are built once the controllers response also arrives. + try: + self._hardware_components = future.result().component + self._build_and_push_dependency_rules() + except Exception as e: + self._node.get_logger().warning( + f"{self._logger_prefix} list_hardware_components failed: {e}") + + def _build_and_push_dependency_rules(self): + """Once both responses are in, infer the rules and hand them to the engine.""" + if self._controller_states is None or self._hardware_components is None: + return + if not self._controller_states: + self._node.get_logger().warning( + f"{self._logger_prefix} No controllers reported by the controller_manager.") + rules = self.infer_dependency_rules(self._controller_states, self._hardware_components) + self._engine.update_dependency_rules(rules) + # TODO: use matched event for this topic as well? That way we know if controller manager dies. # Minor. Currently we catch unexpected transitions in the engine (all components go to finalized) def _activity_callback(self, msg: ControllerManagerActivity): @@ -123,6 +215,8 @@ def _activity_callback(self, msg: ControllerManagerActivity): continue self._cm_components = components + # The CM is visible now, so infer dependencies (here it is guarded to run once). + self.infer_dependencies() self._push_merged_state() def _on_lifecycle_publisher_matched(self, name: str, info: QoSSubscriptionMatchedInfo): diff --git a/foreman/foreman/engine.py b/foreman/foreman/engine.py index d888e1a..b5e7c37 100644 --- a/foreman/foreman/engine.py +++ b/foreman/foreman/engine.py @@ -5,6 +5,7 @@ from foreman.planner import Planner from foreman.types import Component from foreman.types import ComponentType +from foreman.types import ControllerDependencyRule from foreman.types import ErrorSnapshot from foreman.types import ForemanError from foreman.types import ForemanErrorCategory @@ -91,6 +92,11 @@ def abort_goal(self, error: ForemanError): self._last_issued_command = None self._locked_abort_transition() + def update_dependency_rules(self, dependency_rules: List[ControllerDependencyRule]): + """Hand newly inferred dependency rules to the planner.""" + with self._state_lock: + self._planner.replace_dependency_rules(dependency_rules) + def get_next_transition(self) -> Optional[SystemTransitionCommand]: """Calculate the next step toward the goal.""" if not self._current_goal: diff --git a/foreman/foreman/parser.py b/foreman/foreman/parser.py index 098a1c2..48ca5f9 100644 --- a/foreman/foreman/parser.py +++ b/foreman/foreman/parser.py @@ -8,7 +8,6 @@ from foreman.types import Component from foreman.types import ComponentType from foreman.types import ControllerDependencyRule -from foreman.types import HardwareRequirement from foreman.types import LifecycleState from foreman.types import SystemGoal @@ -44,49 +43,6 @@ def parse_state_string(state_str: str) -> LifecycleState: return state_mapping[normalized] -def parse_requires( - requires: List[str], hardware: List[str], lifecycle_nodes: List[str] = None -) -> List[HardwareRequirement]: - """Parse the 'requires' field into list of HardwareRequirement. - - Supports: - - [all, inactive] -> all hardware + lifecycle nodes must be at that state - - [component_name, active] -> specific hardware or lifecycle node must be at that state - """ - if lifecycle_nodes is None: - lifecycle_nodes = [] - - if not requires: - return [] - - # we can get either a single [component, state] entry - # or a list of [component, state] entries. - # here we normalize so we work with a list of [component, state] - if len(requires) == 2 and isinstance(requires[0], str): - requires_normalized = [requires] - else: - requires_normalized = requires - - reqs = [] - - for req in requires_normalized: - if not isinstance(req, list) or len(req) != 2: - raise ValueError(f"Invalid requirement format: {req}. Expected [target, state].") - - target = req[0] - state = parse_state_string(req[1]) - - if target == 'all': - reqs.extend([ - HardwareRequirement(name=name, state=state) - for name in hardware + lifecycle_nodes - ]) - else: - reqs.append(HardwareRequirement(name=target, state=state)) - - return reqs - - def parse_yaml_file(file_path: Path) -> ParsedScenario: """Parse a scenario YAML file into a ParsedScenario object.""" with open(file_path, 'r') as f: @@ -98,16 +54,8 @@ def parse_yaml_file(file_path: Path) -> ParsedScenario: hardware = data.get('hardware', []) lifecycle_nodes = data.get('lifecycle_nodes', []) - dependency_rules = [] - controllers = data.get('controllers', {}) - for ctrl_name, ctrl_config in controllers.items(): - requires = ctrl_config.get('requires', []) - reqs = parse_requires(requires, hardware, lifecycle_nodes) - - dependency_rules.append(ControllerDependencyRule( - controller_name=ctrl_name, - required_hardware=reqs - )) + # Dependencies are inferred at runtime from the controller_manager + dependency_rules: List[ControllerDependencyRule] = [] goals = {} goal_states = data.get('goal_states', {}) @@ -152,8 +100,6 @@ def parse_yaml_file(file_path: Path) -> ParsedScenario: metadata[key] = value tracked_components = set(hardware + lifecycle_nodes) - for rule in dependency_rules: - tracked_components.add(rule.controller_name) for goal in goals.values(): tracked_components.update(c.name for c in goal.hardware_goals) tracked_components.update(c.name for c in goal.controller_goals) diff --git a/foreman/foreman/planner.py b/foreman/foreman/planner.py index ffa6eb9..08c152a 100644 --- a/foreman/foreman/planner.py +++ b/foreman/foreman/planner.py @@ -15,6 +15,10 @@ class Planner: def __init__(self, dependency_rules: List[ControllerDependencyRule]): self.rules = {rule.controller_name: rule for rule in dependency_rules} + def replace_dependency_rules(self, dependency_rules: List[ControllerDependencyRule]): + """Swap the whole rule set at runtime with freshly inferred rules.""" + self.rules = {rule.controller_name: rule for rule in dependency_rules} + def get_next_transition( self, current_state: SystemState, goal: SystemGoal ) -> Optional[SystemTransitionCommand]: diff --git a/foreman/test/test_component_state_monitor.py b/foreman/test/test_component_state_monitor.py new file mode 100644 index 0000000..53daa63 --- /dev/null +++ b/foreman/test/test_component_state_monitor.py @@ -0,0 +1,146 @@ +import unittest +from unittest import mock +from unittest.mock import Mock + +from controller_manager_msgs.msg import ControllerState +from controller_manager_msgs.msg import HardwareComponentState +from controller_manager_msgs.msg import HardwareInterface +from controller_manager_msgs.srv import ListControllers +from controller_manager_msgs.srv import ListHardwareComponents +import rclpy +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.task import Future + +from foreman.adapters.component_state_monitor import ComponentStateMonitor +from foreman.types import LifecycleState + + +def _controller_state(name, required_command=None, required_state=None): + """Build a real ControllerState message, like list_controllers returns.""" + return ControllerState( + name=name, + required_command_interfaces=required_command or [], + required_state_interfaces=required_state or [], + ) + + +def _hardware_component(name, command=None, state=None): + """Build a real HardwareComponentState message, like list_hardware_components returns.""" + return HardwareComponentState( + name=name, + command_interfaces=[HardwareInterface(name=n) for n in (command or [])], + state_interfaces=[HardwareInterface(name=n) for n in (state or [])], + ) + + +class TestInferDependencyRules(unittest.TestCase): + """Pure inference logic: controller required-interfaces to the owning hardware.""" + + def test_maps_controller_to_owning_hardware(self): + controllers = [_controller_state("ctrl_a", required_command=["joint1/position"])] + hardware = [_hardware_component("RRBot", command=["joint1/position"])] + rules = {r.controller_name: r for r in + ComponentStateMonitor.infer_dependency_rules(controllers, hardware)} + deps = [(h.name, h.state) for h in rules["ctrl_a"].required_hardware] + self.assertEqual(deps, [("RRBot", LifecycleState.ACTIVE)]) + + def test_resolves_owner_by_interface_not_by_prefix(self): + # here e.g. 'rrbot_joint1/position' is owned by RRBotSystemPositionOnly, not by 'rrbot_joint1'. + controllers = [_controller_state("pos_ctrl", required_command=["rrbot_joint1/position"])] + hardware = [_hardware_component("RRBotSystemPositionOnly", + command=["rrbot_joint1/position"])] + rules = ComponentStateMonitor.infer_dependency_rules(controllers, hardware) + self.assertEqual([h.name for h in rules[0].required_hardware], ["RRBotSystemPositionOnly"]) + + def test_controller_with_no_owning_hardware_has_no_dependency(self): + controllers = [_controller_state("lonely", required_command=["ghost/iface"])] + hardware = [_hardware_component("RRBot", command=["joint1/position"])] + rules = ComponentStateMonitor.infer_dependency_rules(controllers, hardware) + self.assertEqual(rules[0].required_hardware, []) + + +class TestDependencyInferenceWiring(unittest.TestCase): + """CM service round-trip wiring, with a real node and a mocked engine.""" + + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_component_state_monitor") + # the adapter reads these; the real ForemanNode sets them, a bare node may not + self.node.callback_group_services = MutuallyExclusiveCallbackGroup() + self.node.callback_group_subscriber = ReentrantCallbackGroup() + self.addCleanup(self.node.destroy_node) + self.engine = Mock() + self.monitor = ComponentStateMonitor(self.node, self.engine, "test_controller_manager", []) + + def test_infer_skips_when_services_not_ready(self): + # No CM running, clients not ready then we must not call the services + with mock.patch.object(self.monitor._client_list_controllers, "call_async") as ctrl_call, \ + mock.patch.object(self.monitor._client_list_hardware_components, "call_async") as hw_call: + self.monitor.infer_dependencies() + ctrl_call.assert_not_called() + hw_call.assert_not_called() + self.assertFalse(self.monitor._dependencies_inferred) + + def test_infer_runs_only_once(self): + self.monitor._dependencies_inferred = True + with mock.patch.object(self.monitor._client_list_controllers, "call_async") as ctrl_call: + self.monitor.infer_dependencies() + ctrl_call.assert_not_called() + + def test_rules_not_pushed_until_both_responses_arrive(self): + self.monitor._controller_states = [] # controllers arrived + self.monitor._hardware_components = None # hardware not yet received + self.monitor._build_and_push_dependency_rules() + self.engine.update_dependency_rules.assert_not_called() + + def test_inferred_rules_are_pushed_to_engine(self): + self.monitor._controller_states = [ + _controller_state("ctrl_a", required_command=["joint1/position"])] + self.monitor._hardware_components = [ + _hardware_component("RRBot", command=["joint1/position"])] + self.monitor._build_and_push_dependency_rules() + self.engine.update_dependency_rules.assert_called_once() + rules = self.engine.update_dependency_rules.call_args[0][0] + self.assertEqual(rules[0].controller_name, "ctrl_a") + self.assertEqual(rules[0].required_hardware[0].name, "RRBot") + self.assertEqual(rules[0].required_hardware[0].state, LifecycleState.ACTIVE) + + def test_full_flow_queries_cm_and_pushes_rules(self): + controllers_future = Future() + controllers_resp = ListControllers.Response() + controllers_resp.controller = [ + _controller_state("ctrl_a", required_command=["joint1/position"])] + controllers_future.set_result(controllers_resp) + + hardware_future = Future() + hardware_resp = ListHardwareComponents.Response() + hardware_resp.component = [_hardware_component("RRBot", command=["joint1/position"])] + hardware_future.set_result(hardware_resp) + + with mock.patch.object(self.monitor._client_list_controllers, + "service_is_ready", return_value=True), \ + mock.patch.object(self.monitor._client_list_hardware_components, + "service_is_ready", return_value=True), \ + mock.patch.object(self.monitor._client_list_controllers, + "call_async", return_value=controllers_future), \ + mock.patch.object(self.monitor._client_list_hardware_components, + "call_async", return_value=hardware_future): + self.monitor.infer_dependencies() + + self.assertTrue(self.monitor._dependencies_inferred) + self.engine.update_dependency_rules.assert_called_once() + rules = self.engine.update_dependency_rules.call_args[0][0] + self.assertEqual(rules[0].controller_name, "ctrl_a") + self.assertEqual(rules[0].required_hardware[0].name, "RRBot") + + +if __name__ == '__main__': + unittest.main() diff --git a/foreman/test/test_engine.py b/foreman/test/test_engine.py index 15bb366..5d18b00 100644 --- a/foreman/test/test_engine.py +++ b/foreman/test/test_engine.py @@ -6,8 +6,10 @@ from foreman.parser import ParsedScenario from foreman.types import Component from foreman.types import ComponentType +from foreman.types import ControllerDependencyRule from foreman.types import ForemanError from foreman.types import ForemanErrorCategory +from foreman.types import HardwareRequirement from foreman.types import LifecycleState from foreman.types import SystemGoal @@ -278,3 +280,48 @@ def test_goal_accepted_when_dependency_already_satisfied(dependency_config): response = engine.request_goal('active') assert response.success is True + + +@pytest.fixture +def inferred_rules_config(): + """Goal wants 'gripper' active, but the config declares no dependency rules.""" + + goal = SystemGoal('run', + controller_goals=[Component('gripper', ComponentType.CONTROLLER, LifecycleState.ACTIVE)]) + return ParsedScenario( + hardware=["hw1"], + dependency_rules=[], + goals={'run': goal}, + tracked_components={"hw1", "gripper"} + ) + + +def test_inferred_rules_change_what_the_planner_allows(inferred_rules_config): + """Rules fed at runtime must steer planning: same state, different decision.""" + + lock = threading.Lock() + engine = ForemanEngine(inferred_rules_config, lock) + + # hw1 is only INACTIVE, gripper is INACTIVE, and no rules are known yet. + engine.set_system_state([ + Component('hw1', ComponentType.HARDWARE, LifecycleState.INACTIVE), + Component('gripper', ComponentType.CONTROLLER, LifecycleState.INACTIVE), + ]) + engine.request_goal('run') + + # No rules yet, then planner activates gripper + cmd = engine.get_next_transition() + assert cmd is not None + assert cmd.component.name == 'gripper' + assert cmd.goal_state == LifecycleState.ACTIVE + + # Feed in the inferred rule "gripper needs hw1 ACTIVE" (it is only INACTIVE). + engine.update_dependency_rules([ + ControllerDependencyRule( + controller_name='gripper', + required_hardware=[HardwareRequirement('hw1', LifecycleState.ACTIVE)] + ) + ]) + + # Same observed state, but now gripper is blocked and hw1 isn't in the goal -> no move. + assert engine.get_next_transition() is None diff --git a/foreman/test/test_parser.py b/foreman/test/test_parser.py index a4aeaf0..da2dca7 100644 --- a/foreman/test/test_parser.py +++ b/foreman/test/test_parser.py @@ -6,8 +6,6 @@ from foreman.parser import ParsedScenario from foreman.types import Component from foreman.types import ComponentType -from foreman.types import ControllerDependencyRule -from foreman.types import HardwareRequirement from foreman.types import LifecycleState from foreman.types import SystemGoal @@ -36,40 +34,9 @@ def test_lifecycle_nodes_list(self, parsed_scenario): def test_metadata_empty(self, parsed_scenario): assert parsed_scenario.metadata == {} - -class TestDependencyRules: - """Tests for parsed dependency rules.""" - - def test_rules_count(self, parsed_scenario): - assert len(parsed_scenario.dependency_rules) == 3 - - def test_joint_state_broadcaster_rule(self, parsed_scenario): - rule = next(r for r in parsed_scenario.dependency_rules if r.controller_name == - "joint_state_broadcaster") - assert rule.controller_name == "joint_state_broadcaster" - assert len(rule.required_hardware) == 3 - reqs_by_name = {req.name: req for req in rule.required_hardware} - assert set(reqs_by_name.keys()) == { - "FrankaHardwareInterface", "kassow", "dummy_lifecycle_node"} - assert reqs_by_name["kassow"].state == LifecycleState.INACTIVE - assert reqs_by_name["FrankaHardwareInterface"].state == LifecycleState.INACTIVE - assert reqs_by_name["dummy_lifecycle_node"].state == LifecycleState.ACTIVE - - def test_kassow_jtc_rule(self, parsed_scenario): - rule = next(r for r in parsed_scenario.dependency_rules if r.controller_name == - "kassow_joint_trajectory_controller") - assert rule.controller_name == "kassow_joint_trajectory_controller" - assert len(rule.required_hardware) == 1 - assert rule.required_hardware[0].name == "kassow" - assert rule.required_hardware[0].state == LifecycleState.ACTIVE - - def test_franka_jtc_rule(self, parsed_scenario): - rule = next(r for r in parsed_scenario.dependency_rules if r.controller_name == - "franka_joint_trajectory_controller") - assert rule.controller_name == "franka_joint_trajectory_controller" - assert len(rule.required_hardware) == 1 - assert rule.required_hardware[0].name == "FrankaHardwareInterface" - assert rule.required_hardware[0].state == LifecycleState.ACTIVE + def test_dependency_rules_not_parsed(self, parsed_scenario): + """Dependencies are inferred at runtime, never parsed from YAML.""" + assert parsed_scenario.dependency_rules == [] class TestGoalStates: diff --git a/foreman/test/test_planner.py b/foreman/test/test_planner.py index acdfba8..f36cdbf 100644 --- a/foreman/test/test_planner.py +++ b/foreman/test/test_planner.py @@ -55,6 +55,21 @@ def apply_command(state: SystemState, cmd: SystemTransitionCommand): ) +def test_replace_dependency_rules_swaps_whole_set(basic_planner): + """replace_dependency_rules drops the old rules and installs the new ones.""" + assert 'franka_jtc' in basic_planner.rules + + basic_planner.replace_dependency_rules([ + ControllerDependencyRule( + controller_name='new_ctrl', + required_hardware=[HardwareRequirement('new_hw', LifecycleState.ACTIVE)] + ) + ]) + + assert 'franka_jtc' not in basic_planner.rules + assert basic_planner.rules['new_ctrl'].required_hardware[0].name == 'new_hw' + + def test_scenario_1_standard_bring_up(basic_planner): state = SystemState(components={ 'franka_hw': Component('franka_hw', ComponentType.HARDWARE, LifecycleState.UNCONFIGURED), From 98cd48dc17ff34aaa0769c9183dfaf043488b44a Mon Sep 17 00:00:00 2001 From: root Date: Wed, 24 Jun 2026 06:26:41 +0000 Subject: [PATCH 2/8] Address review feedback for dependency inference Signed-off-by: root --- .../adapters/component_state_monitor.py | 95 ++++++----- foreman/foreman/engine.py | 6 +- foreman/foreman/planner.py | 4 +- foreman/test/test_component_state_monitor.py | 148 ++++++++++-------- foreman/test/test_engine.py | 2 +- foreman/test/test_planner.py | 6 +- 6 files changed, 133 insertions(+), 128 deletions(-) diff --git a/foreman/foreman/adapters/component_state_monitor.py b/foreman/foreman/adapters/component_state_monitor.py index 980dc27..6b59966 100644 --- a/foreman/foreman/adapters/component_state_monitor.py +++ b/foreman/foreman/adapters/component_state_monitor.py @@ -68,7 +68,7 @@ def __init__( f"{self._logger_prefix} Subscribed to /{controller_manager_name}/activity" ) - # Service clients used to read the CM's controllers and hardware once, to infer dependencies. + # Service clients used to read the CM's controllers and hardware to infer dependencies. self._client_list_controllers = self._node.create_client( ListControllers, f'/{controller_manager_name}/list_controllers', @@ -79,11 +79,6 @@ def __init__( f'/{controller_manager_name}/list_hardware_components', callback_group=self._node.callback_group_services ) - # The two responses arrive separately, here None means "not received yet". - self._controller_states = None - self._hardware_components = None - # Inference runs only once, the first time the CM becomes visible. - self._dependencies_inferred = False # --- Lifecycle node monitoring --- self._lc_node_get_state_clients: Dict[str, object] = {} @@ -119,74 +114,72 @@ def __init__( @staticmethod def infer_dependency_rules(controller_states, hardware_components): - """Map each controller to the hardware that owns the interfaces it requires.""" - interface_owner = {} + """Infer each controller's hardware dependencies from its required interfaces. + + A command interface requires its owning hardware ACTIVE; a state interface + requires it only INACTIVE. If a controller needs both from one hardware, ACTIVE wins. + """ + owner_of_interface = {} + # Build a mapping of interface names to their owning hardware components for hardware in hardware_components: for interface in list(hardware.command_interfaces) + list(hardware.state_interfaces): - interface_owner[interface.name] = hardware.name + owner_of_interface[interface.name] = hardware.name dependency_rules = [] for controller in controller_states: - required_interfaces = ( - set(controller.required_command_interfaces) - | set(controller.required_state_interfaces) - ) - # Resolve each interface to the hardware that owns it. - required_hardware_names = { - interface_owner[name] for name in required_interfaces if name in interface_owner - } + # Map required interfaces to hardware components and their required lifecycle states + required_hardware_state = {} + for interface in controller.required_command_interfaces: + hardware_name = owner_of_interface.get(interface) + if hardware_name: + required_hardware_state[hardware_name] = LifecycleState.ACTIVE + + for interface in controller.required_state_interfaces: + hardware_name = owner_of_interface.get(interface) + if hardware_name and hardware_name not in required_hardware_state: + required_hardware_state[hardware_name] = LifecycleState.INACTIVE + dependency_rules.append(ControllerDependencyRule( controller_name=controller.name, required_hardware=[ - HardwareRequirement(name=hardware_name, state=LifecycleState.ACTIVE) - for hardware_name in required_hardware_names + HardwareRequirement(name=name, state=state) + for name, state in required_hardware_state.items() ], )) return dependency_rules - def infer_dependencies(self): - """Ask the controller_manager (once) for its controllers and hardware to infer dependencies.""" - if self._dependencies_inferred: - return + def _refresh_dependency_rules(self): + """Refresh dependency rules based on latest controller and hardware data. Runs on every activity update.""" if not (self._client_list_controllers.service_is_ready() and self._client_list_hardware_components.service_is_ready()): return - self._dependencies_inferred = True - controllers_future = self._client_list_controllers.call_async(ListControllers.Request()) controllers_future.add_done_callback(self._on_list_controllers_response) - hardware_future = self._client_list_hardware_components.call_async( - ListHardwareComponents.Request()) - hardware_future.add_done_callback(self._on_list_hardware_components_response) - def _on_list_controllers_response(self, future): - # Cache the controllers; rules are built once the hardware response also arrives. + """Read the controllers, then request the hardware list (built in the next callback).""" try: - self._controller_states = future.result().controller - self._build_and_push_dependency_rules() - except Exception as e: + controllers = future.result().controller + except Exception as error: self._node.get_logger().warning( - f"{self._logger_prefix} list_controllers failed: {e}") + f"{self._logger_prefix} list_controllers failed: {error}") + return + hardware_future = self._client_list_hardware_components.call_async( + ListHardwareComponents.Request()) + # Carry the controllers into the next callback so it has both lists to build the rules. + hardware_future.add_done_callback( + lambda future: self._on_list_hardware_components_response(future, controllers)) - def _on_list_hardware_components_response(self, future): - # Cache the hardware; rules are built once the controllers response also arrives. + def _on_list_hardware_components_response(self, future, controllers): + """Hardware components received. Now we build the dependency rules and pass them to the engine.""" try: - self._hardware_components = future.result().component - self._build_and_push_dependency_rules() - except Exception as e: + hardware_components = future.result().component + except Exception as error: self._node.get_logger().warning( - f"{self._logger_prefix} list_hardware_components failed: {e}") - - def _build_and_push_dependency_rules(self): - """Once both responses are in, infer the rules and hand them to the engine.""" - if self._controller_states is None or self._hardware_components is None: + f"{self._logger_prefix} list_hardware_components failed: {error}") return - if not self._controller_states: - self._node.get_logger().warning( - f"{self._logger_prefix} No controllers reported by the controller_manager.") - rules = self.infer_dependency_rules(self._controller_states, self._hardware_components) - self._engine.update_dependency_rules(rules) + rules = self.infer_dependency_rules(controllers, hardware_components) + self._engine.set_dependency_rules(rules) # TODO: use matched event for this topic as well? That way we know if controller manager dies. # Minor. Currently we catch unexpected transitions in the engine (all components go to finalized) @@ -215,8 +208,8 @@ def _activity_callback(self, msg: ControllerManagerActivity): continue self._cm_components = components - # The CM is visible now, so infer dependencies (here it is guarded to run once). - self.infer_dependencies() + # Refresh dependency rules based on the latest controller and hardware data. + self._refresh_dependency_rules() self._push_merged_state() def _on_lifecycle_publisher_matched(self, name: str, info: QoSSubscriptionMatchedInfo): diff --git a/foreman/foreman/engine.py b/foreman/foreman/engine.py index b5e7c37..caaf109 100644 --- a/foreman/foreman/engine.py +++ b/foreman/foreman/engine.py @@ -92,10 +92,10 @@ def abort_goal(self, error: ForemanError): self._last_issued_command = None self._locked_abort_transition() - def update_dependency_rules(self, dependency_rules: List[ControllerDependencyRule]): - """Hand newly inferred dependency rules to the planner.""" + def set_dependency_rules(self, dependency_rules: List[ControllerDependencyRule]): + """Update planner with new dependency rules.""" with self._state_lock: - self._planner.replace_dependency_rules(dependency_rules) + self._planner.set_dependency_rules(dependency_rules) def get_next_transition(self) -> Optional[SystemTransitionCommand]: """Calculate the next step toward the goal.""" diff --git a/foreman/foreman/planner.py b/foreman/foreman/planner.py index 08c152a..002cbc6 100644 --- a/foreman/foreman/planner.py +++ b/foreman/foreman/planner.py @@ -15,8 +15,8 @@ class Planner: def __init__(self, dependency_rules: List[ControllerDependencyRule]): self.rules = {rule.controller_name: rule for rule in dependency_rules} - def replace_dependency_rules(self, dependency_rules: List[ControllerDependencyRule]): - """Swap the whole rule set at runtime with freshly inferred rules.""" + def set_dependency_rules(self, dependency_rules: List[ControllerDependencyRule]): + """Update the controller dependency rules.""" self.rules = {rule.controller_name: rule for rule in dependency_rules} def get_next_transition( diff --git a/foreman/test/test_component_state_monitor.py b/foreman/test/test_component_state_monitor.py index 53daa63..51cd968 100644 --- a/foreman/test/test_component_state_monitor.py +++ b/foreman/test/test_component_state_monitor.py @@ -17,7 +17,7 @@ def _controller_state(name, required_command=None, required_state=None): - """Build a real ControllerState message, like list_controllers returns.""" + """Build a ControllerState message like list_controllers returns.""" return ControllerState( name=name, required_command_interfaces=required_command or [], @@ -26,7 +26,7 @@ def _controller_state(name, required_command=None, required_state=None): def _hardware_component(name, command=None, state=None): - """Build a real HardwareComponentState message, like list_hardware_components returns.""" + """Build a HardwareComponentState message like list_hardware_components returns.""" return HardwareComponentState( name=name, command_interfaces=[HardwareInterface(name=n) for n in (command or [])], @@ -35,33 +35,55 @@ def _hardware_component(name, command=None, state=None): class TestInferDependencyRules(unittest.TestCase): - """Pure inference logic: controller required-interfaces to the owning hardware.""" + """Test the inference of dependency rules based on controller and hardware states.""" - def test_maps_controller_to_owning_hardware(self): - controllers = [_controller_state("ctrl_a", required_command=["joint1/position"])] + def test_command_interface_requires_hardware_active(self): + # A controller that writes a command interface needs its hardware ACTIVE. + controllers = [_controller_state( + "forward_position_controller", required_command=["joint1/position"])] hardware = [_hardware_component("RRBot", command=["joint1/position"])] - rules = {r.controller_name: r for r in - ComponentStateMonitor.infer_dependency_rules(controllers, hardware)} - deps = [(h.name, h.state) for h in rules["ctrl_a"].required_hardware] - self.assertEqual(deps, [("RRBot", LifecycleState.ACTIVE)]) - - def test_resolves_owner_by_interface_not_by_prefix(self): - # here e.g. 'rrbot_joint1/position' is owned by RRBotSystemPositionOnly, not by 'rrbot_joint1'. + rule = ComponentStateMonitor.infer_dependency_rules(controllers, hardware)[0] + self.assertEqual(rule.required_hardware[0].name, "RRBot") + self.assertEqual(rule.required_hardware[0].state, LifecycleState.ACTIVE) + + def test_state_interface_requires_hardware_inactive(self): + # A broadcaster only reads a state interface, so the hardware need only be INACTIVE. + controllers = [_controller_state("joint_state_broadcaster", + required_state=["joint1/position"])] + hardware = [_hardware_component("RRBot", state=["joint1/position"])] + rule = ComponentStateMonitor.infer_dependency_rules(controllers, hardware)[0] + self.assertEqual(rule.required_hardware[0].name, "RRBot") + self.assertEqual(rule.required_hardware[0].state, LifecycleState.INACTIVE) + + def test_needing_command_and_state_requires_active(self): + # If a controller needs both kinds from one hardware, the strict ACTIVE requirement wins. + controllers = [_controller_state("mixed", + required_command=["joint1/position"], + required_state=["joint1/velocity"])] + hardware = [_hardware_component("RRBot", + command=["joint1/position"], + state=["joint1/velocity"])] + rule = ComponentStateMonitor.infer_dependency_rules(controllers, hardware)[0] + self.assertEqual(rule.required_hardware[0].state, LifecycleState.ACTIVE) + + def test_owner_resolved_by_interface_name_not_prefix(self): + # rrbot_joint1/position is owned by whichever hardware exports it, not by rrbot_joint1. controllers = [_controller_state("pos_ctrl", required_command=["rrbot_joint1/position"])] hardware = [_hardware_component("RRBotSystemPositionOnly", command=["rrbot_joint1/position"])] - rules = ComponentStateMonitor.infer_dependency_rules(controllers, hardware) - self.assertEqual([h.name for h in rules[0].required_hardware], ["RRBotSystemPositionOnly"]) + rule = ComponentStateMonitor.infer_dependency_rules(controllers, hardware)[0] + self.assertEqual([h.name for h in rule.required_hardware], ["RRBotSystemPositionOnly"]) - def test_controller_with_no_owning_hardware_has_no_dependency(self): + def test_interface_with_no_owning_hardware_gives_no_dependency(self): + # An interface no hardware exports (e.g. hardware not up yet) yields no dependency. controllers = [_controller_state("lonely", required_command=["ghost/iface"])] hardware = [_hardware_component("RRBot", command=["joint1/position"])] - rules = ComponentStateMonitor.infer_dependency_rules(controllers, hardware) - self.assertEqual(rules[0].required_hardware, []) + rule = ComponentStateMonitor.infer_dependency_rules(controllers, hardware)[0] + self.assertEqual(rule.required_hardware, []) class TestDependencyInferenceWiring(unittest.TestCase): - """CM service round-trip wiring, with a real node and a mocked engine.""" + """The service round-trip: read the CM on each activity update and set the rules on the engine.""" @classmethod def setUpClass(cls): @@ -73,57 +95,20 @@ def tearDownClass(cls): def setUp(self): self.node = rclpy.create_node("test_component_state_monitor") - # the adapter reads these; the real ForemanNode sets them, a bare node may not + # ComponentStateMonitor expects these callback groups on its node; a real ForemanNode + # creates them, so we add them to this plain test node. self.node.callback_group_services = MutuallyExclusiveCallbackGroup() self.node.callback_group_subscriber = ReentrantCallbackGroup() self.addCleanup(self.node.destroy_node) self.engine = Mock() self.monitor = ComponentStateMonitor(self.node, self.engine, "test_controller_manager", []) - def test_infer_skips_when_services_not_ready(self): - # No CM running, clients not ready then we must not call the services - with mock.patch.object(self.monitor._client_list_controllers, "call_async") as ctrl_call, \ - mock.patch.object(self.monitor._client_list_hardware_components, "call_async") as hw_call: - self.monitor.infer_dependencies() - ctrl_call.assert_not_called() - hw_call.assert_not_called() - self.assertFalse(self.monitor._dependencies_inferred) - - def test_infer_runs_only_once(self): - self.monitor._dependencies_inferred = True - with mock.patch.object(self.monitor._client_list_controllers, "call_async") as ctrl_call: - self.monitor.infer_dependencies() - ctrl_call.assert_not_called() - - def test_rules_not_pushed_until_both_responses_arrive(self): - self.monitor._controller_states = [] # controllers arrived - self.monitor._hardware_components = None # hardware not yet received - self.monitor._build_and_push_dependency_rules() - self.engine.update_dependency_rules.assert_not_called() - - def test_inferred_rules_are_pushed_to_engine(self): - self.monitor._controller_states = [ - _controller_state("ctrl_a", required_command=["joint1/position"])] - self.monitor._hardware_components = [ - _hardware_component("RRBot", command=["joint1/position"])] - self.monitor._build_and_push_dependency_rules() - self.engine.update_dependency_rules.assert_called_once() - rules = self.engine.update_dependency_rules.call_args[0][0] - self.assertEqual(rules[0].controller_name, "ctrl_a") - self.assertEqual(rules[0].required_hardware[0].name, "RRBot") - self.assertEqual(rules[0].required_hardware[0].state, LifecycleState.ACTIVE) - - def test_full_flow_queries_cm_and_pushes_rules(self): + def _infer_with(self, controllers, hardware): + # Mock the controller manager services to return the given controllers and hardware, then refresh rules. controllers_future = Future() - controllers_resp = ListControllers.Response() - controllers_resp.controller = [ - _controller_state("ctrl_a", required_command=["joint1/position"])] - controllers_future.set_result(controllers_resp) - + controllers_future.set_result(ListControllers.Response(controller=controllers)) hardware_future = Future() - hardware_resp = ListHardwareComponents.Response() - hardware_resp.component = [_hardware_component("RRBot", command=["joint1/position"])] - hardware_future.set_result(hardware_resp) + hardware_future.set_result(ListHardwareComponents.Response(component=hardware)) with mock.patch.object(self.monitor._client_list_controllers, "service_is_ready", return_value=True), \ @@ -133,13 +118,40 @@ def test_full_flow_queries_cm_and_pushes_rules(self): "call_async", return_value=controllers_future), \ mock.patch.object(self.monitor._client_list_hardware_components, "call_async", return_value=hardware_future): - self.monitor.infer_dependencies() - - self.assertTrue(self.monitor._dependencies_inferred) - self.engine.update_dependency_rules.assert_called_once() - rules = self.engine.update_dependency_rules.call_args[0][0] - self.assertEqual(rules[0].controller_name, "ctrl_a") - self.assertEqual(rules[0].required_hardware[0].name, "RRBot") + self.monitor._refresh_dependency_rules() + + def test_refresh_skips_when_services_not_ready(self): + # No controller_manager is up, so its clients are not ready, we do nothing. + with mock.patch.object(self.monitor._client_list_controllers, "call_async") as list_call: + self.monitor._refresh_dependency_rules() + list_call.assert_not_called() + self.engine.set_dependency_rules.assert_not_called() + + def test_refresh_reads_cm_and_sets_rules_on_engine(self): + self._infer_with( + controllers=[_controller_state("forward_position_controller", + required_command=["joint1/position"])], + hardware=[_hardware_component("RRBot", command=["joint1/position"])]) + + self.engine.set_dependency_rules.assert_called_once() + rule = self.engine.set_dependency_rules.call_args[0][0][0] + self.assertEqual(rule.controller_name, "forward_position_controller") + self.assertEqual(rule.required_hardware[0].name, "RRBot") + self.assertEqual(rule.required_hardware[0].state, LifecycleState.ACTIVE) + + def test_refresh_runs_again_on_next_activity(self): + # There is no run-once guard, each activity update re-infers and sets the dependency rules again. + controllers = [_controller_state("forward_position_controller", + required_command=["joint1/position"])] + hardware = [_hardware_component("RRBot", command=["joint1/position"])] + self._infer_with(controllers=controllers, hardware=hardware) + self._infer_with(controllers=controllers, hardware=hardware) + self.assertEqual(self.engine.set_dependency_rules.call_count, 2) + + def test_no_controllers_yields_empty_rules(self): + # No controllers yet, empty rule set. + self._infer_with(controllers=[], hardware=[]) + self.engine.set_dependency_rules.assert_called_once_with([]) if __name__ == '__main__': diff --git a/foreman/test/test_engine.py b/foreman/test/test_engine.py index 5d18b00..4aaa0c3 100644 --- a/foreman/test/test_engine.py +++ b/foreman/test/test_engine.py @@ -316,7 +316,7 @@ def test_inferred_rules_change_what_the_planner_allows(inferred_rules_config): assert cmd.goal_state == LifecycleState.ACTIVE # Feed in the inferred rule "gripper needs hw1 ACTIVE" (it is only INACTIVE). - engine.update_dependency_rules([ + engine.set_dependency_rules([ ControllerDependencyRule( controller_name='gripper', required_hardware=[HardwareRequirement('hw1', LifecycleState.ACTIVE)] diff --git a/foreman/test/test_planner.py b/foreman/test/test_planner.py index f36cdbf..6c3fbf6 100644 --- a/foreman/test/test_planner.py +++ b/foreman/test/test_planner.py @@ -55,11 +55,11 @@ def apply_command(state: SystemState, cmd: SystemTransitionCommand): ) -def test_replace_dependency_rules_swaps_whole_set(basic_planner): - """replace_dependency_rules drops the old rules and installs the new ones.""" +def test_set_dependency_rules_swaps_whole_set(basic_planner): + """set_dependency_rules drops the old rules and installs the new ones.""" assert 'franka_jtc' in basic_planner.rules - basic_planner.replace_dependency_rules([ + basic_planner.set_dependency_rules([ ControllerDependencyRule( controller_name='new_ctrl', required_hardware=[HardwareRequirement('new_hw', LifecycleState.ACTIVE)] From 27cea7f1d329c63834bf46e7704feefb27c0e5fb Mon Sep 17 00:00:00 2001 From: root Date: Fri, 26 Jun 2026 03:56:31 +0000 Subject: [PATCH 3/8] Refactor dependency rule queries --- .../adapters/component_state_monitor.py | 49 ++++---------- foreman/foreman/engine.py | 10 +-- foreman/foreman/node.py | 2 + foreman/foreman/planner.py | 24 +++++-- foreman/test/test_component_state_monitor.py | 64 ++++++------------- foreman/test/test_engine.py | 32 ++++++---- foreman/test/test_planner.py | 27 ++++---- 7 files changed, 93 insertions(+), 115 deletions(-) diff --git a/foreman/foreman/adapters/component_state_monitor.py b/foreman/foreman/adapters/component_state_monitor.py index 6b59966..396989a 100644 --- a/foreman/foreman/adapters/component_state_monitor.py +++ b/foreman/foreman/adapters/component_state_monitor.py @@ -68,16 +68,16 @@ def __init__( f"{self._logger_prefix} Subscribed to /{controller_manager_name}/activity" ) - # Service clients used to read the CM's controllers and hardware to infer dependencies. + # Service clients used to query controller_manager when building dependency rules. self._client_list_controllers = self._node.create_client( ListControllers, f'/{controller_manager_name}/list_controllers', - callback_group=self._node.callback_group_services + callback_group=self._node.callback_group_subscriber ) self._client_list_hardware_components = self._node.create_client( ListHardwareComponents, f'/{controller_manager_name}/list_hardware_components', - callback_group=self._node.callback_group_services + callback_group=self._node.callback_group_subscriber ) # --- Lifecycle node monitoring --- @@ -148,38 +148,19 @@ def infer_dependency_rules(controller_states, hardware_components): )) return dependency_rules - def _refresh_dependency_rules(self): - """Refresh dependency rules based on latest controller and hardware data. Runs on every activity update.""" + def get_dependency_rules(self): + """ + Query the controller_manager and build the current dependency rules. + + Returns [] until the controller_manager services are up. + """ if not (self._client_list_controllers.service_is_ready() and self._client_list_hardware_components.service_is_ready()): - return - controllers_future = self._client_list_controllers.call_async(ListControllers.Request()) - controllers_future.add_done_callback(self._on_list_controllers_response) - - def _on_list_controllers_response(self, future): - """Read the controllers, then request the hardware list (built in the next callback).""" - try: - controllers = future.result().controller - except Exception as error: - self._node.get_logger().warning( - f"{self._logger_prefix} list_controllers failed: {error}") - return - hardware_future = self._client_list_hardware_components.call_async( - ListHardwareComponents.Request()) - # Carry the controllers into the next callback so it has both lists to build the rules. - hardware_future.add_done_callback( - lambda future: self._on_list_hardware_components_response(future, controllers)) - - def _on_list_hardware_components_response(self, future, controllers): - """Hardware components received. Now we build the dependency rules and pass them to the engine.""" - try: - hardware_components = future.result().component - except Exception as error: - self._node.get_logger().warning( - f"{self._logger_prefix} list_hardware_components failed: {error}") - return - rules = self.infer_dependency_rules(controllers, hardware_components) - self._engine.set_dependency_rules(rules) + return [] + controllers = self._client_list_controllers.call(ListControllers.Request()).controller + hardware_components = self._client_list_hardware_components.call( + ListHardwareComponents.Request()).component + return self.infer_dependency_rules(controllers, hardware_components) # TODO: use matched event for this topic as well? That way we know if controller manager dies. # Minor. Currently we catch unexpected transitions in the engine (all components go to finalized) @@ -208,8 +189,6 @@ def _activity_callback(self, msg: ControllerManagerActivity): continue self._cm_components = components - # Refresh dependency rules based on the latest controller and hardware data. - self._refresh_dependency_rules() self._push_merged_state() def _on_lifecycle_publisher_matched(self, name: str, info: QoSSubscriptionMatchedInfo): diff --git a/foreman/foreman/engine.py b/foreman/foreman/engine.py index caaf109..bdb9a45 100644 --- a/foreman/foreman/engine.py +++ b/foreman/foreman/engine.py @@ -92,10 +92,9 @@ def abort_goal(self, error: ForemanError): self._last_issued_command = None self._locked_abort_transition() - def set_dependency_rules(self, dependency_rules: List[ControllerDependencyRule]): - """Update planner with new dependency rules.""" - with self._state_lock: - self._planner.set_dependency_rules(dependency_rules) + def set_dependency_provider(self, dependency_provider): + """Set the source of dependency rules for the planner.""" + self._planner.set_dependency_provider(dependency_provider) def get_next_transition(self) -> Optional[SystemTransitionCommand]: """Calculate the next step toward the goal.""" @@ -257,8 +256,9 @@ def _locked_check_unsatisfiable_dependencies(self, goal: SystemGoal) -> List[str goal_infrastructure_states[comp.name] = comp.lifecycle_state errors = [] + current_dependency_rules = self._planner.get_current_rules() for ctrl_goal in goal.controller_goals: - rule = self._planner.rules.get(ctrl_goal.name) + rule = current_dependency_rules.get(ctrl_goal.name) if not rule: continue diff --git a/foreman/foreman/node.py b/foreman/foreman/node.py index 0194ef0..466bf7b 100644 --- a/foreman/foreman/node.py +++ b/foreman/foreman/node.py @@ -47,6 +47,8 @@ def __init__(self): controller_manager_name=controller_manager_name, lifecycle_nodes=self.foreman_config.lifecycle_nodes ) + # Planner queries dependency rules from the monitor when planning. + self.foreman_engine.set_dependency_provider(self.component_state_monitor) self.controller_manager_service_caller = adapters.ControllerManagerServiceCaller( node=self, controller_manager_name=controller_manager_name diff --git a/foreman/foreman/planner.py b/foreman/foreman/planner.py index 002cbc6..991bddc 100644 --- a/foreman/foreman/planner.py +++ b/foreman/foreman/planner.py @@ -12,12 +12,21 @@ class Planner: """Plan the next single step towards the lifecycle state goal of the system.""" - def __init__(self, dependency_rules: List[ControllerDependencyRule]): - self.rules = {rule.controller_name: rule for rule in dependency_rules} - - def set_dependency_rules(self, dependency_rules: List[ControllerDependencyRule]): - """Update the controller dependency rules.""" - self.rules = {rule.controller_name: rule for rule in dependency_rules} + def __init__(self, dependency_rules: List[ControllerDependencyRule] = None, + dependency_provider=None): + self._dependency_provider = dependency_provider + self.rules = {rule.controller_name: rule for rule in (dependency_rules or [])} + + def set_dependency_provider(self, dependency_provider): + """Set a provider for dependency rules, which can be queried at runtime.""" + self._dependency_provider = dependency_provider + + def get_current_rules(self): + """Return the current dependency rules keyed by controller name.""" + if self._dependency_provider is not None: + self.rules = {rule.controller_name: rule + for rule in self._dependency_provider.get_dependency_rules()} + return self.rules def get_next_transition( self, current_state: SystemState, goal: SystemGoal @@ -27,6 +36,9 @@ def get_next_transition( Priority: C deactivate > HW Down > HW Up > C cleanup > C config > C activate. """ + # Refresh dependency rules from the provider before computing the next transition. + self.get_current_rules() + cmds_hw_step_up = [] cmds_hw_step_down = [] cmds_ctrl_config = [] diff --git a/foreman/test/test_component_state_monitor.py b/foreman/test/test_component_state_monitor.py index 51cd968..de59488 100644 --- a/foreman/test/test_component_state_monitor.py +++ b/foreman/test/test_component_state_monitor.py @@ -10,7 +10,6 @@ import rclpy from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.task import Future from foreman.adapters.component_state_monitor import ComponentStateMonitor from foreman.types import LifecycleState @@ -82,8 +81,8 @@ def test_interface_with_no_owning_hardware_gives_no_dependency(self): self.assertEqual(rule.required_hardware, []) -class TestDependencyInferenceWiring(unittest.TestCase): - """The service round-trip: read the CM on each activity update and set the rules on the engine.""" +class TestDependencyRulesQuery(unittest.TestCase): + """Tests querying dependency rules from the controller_manager.""" @classmethod def setUpClass(cls): @@ -103,55 +102,32 @@ def setUp(self): self.engine = Mock() self.monitor = ComponentStateMonitor(self.node, self.engine, "test_controller_manager", []) - def _infer_with(self, controllers, hardware): - # Mock the controller manager services to return the given controllers and hardware, then refresh rules. - controllers_future = Future() - controllers_future.set_result(ListControllers.Response(controller=controllers)) - hardware_future = Future() - hardware_future.set_result(ListHardwareComponents.Response(component=hardware)) + def test_returns_empty_when_services_not_ready(self): + # When controller_manager services are unavailable, do not attempt any queries. + with mock.patch.object(self.monitor._client_list_controllers, "call") as controllers_call: + self.assertEqual(self.monitor.get_dependency_rules(), []) + controllers_call.assert_not_called() + + def test_queries_cm_and_builds_rules(self): + controllers_response = ListControllers.Response(controller=[ + _controller_state("forward_position_controller", required_command=["joint1/position"])]) + hardware_response = ListHardwareComponents.Response(component=[ + _hardware_component("RRBot", command=["joint1/position"])]) with mock.patch.object(self.monitor._client_list_controllers, "service_is_ready", return_value=True), \ mock.patch.object(self.monitor._client_list_hardware_components, "service_is_ready", return_value=True), \ mock.patch.object(self.monitor._client_list_controllers, - "call_async", return_value=controllers_future), \ + "call", return_value=controllers_response), \ mock.patch.object(self.monitor._client_list_hardware_components, - "call_async", return_value=hardware_future): - self.monitor._refresh_dependency_rules() - - def test_refresh_skips_when_services_not_ready(self): - # No controller_manager is up, so its clients are not ready, we do nothing. - with mock.patch.object(self.monitor._client_list_controllers, "call_async") as list_call: - self.monitor._refresh_dependency_rules() - list_call.assert_not_called() - self.engine.set_dependency_rules.assert_not_called() - - def test_refresh_reads_cm_and_sets_rules_on_engine(self): - self._infer_with( - controllers=[_controller_state("forward_position_controller", - required_command=["joint1/position"])], - hardware=[_hardware_component("RRBot", command=["joint1/position"])]) - - self.engine.set_dependency_rules.assert_called_once() - rule = self.engine.set_dependency_rules.call_args[0][0][0] - self.assertEqual(rule.controller_name, "forward_position_controller") - self.assertEqual(rule.required_hardware[0].name, "RRBot") - self.assertEqual(rule.required_hardware[0].state, LifecycleState.ACTIVE) + "call", return_value=hardware_response): + rules = self.monitor.get_dependency_rules() - def test_refresh_runs_again_on_next_activity(self): - # There is no run-once guard, each activity update re-infers and sets the dependency rules again. - controllers = [_controller_state("forward_position_controller", - required_command=["joint1/position"])] - hardware = [_hardware_component("RRBot", command=["joint1/position"])] - self._infer_with(controllers=controllers, hardware=hardware) - self._infer_with(controllers=controllers, hardware=hardware) - self.assertEqual(self.engine.set_dependency_rules.call_count, 2) - - def test_no_controllers_yields_empty_rules(self): - # No controllers yet, empty rule set. - self._infer_with(controllers=[], hardware=[]) - self.engine.set_dependency_rules.assert_called_once_with([]) + self.assertEqual(len(rules), 1) + self.assertEqual(rules[0].controller_name, "forward_position_controller") + self.assertEqual(rules[0].required_hardware[0].name, "RRBot") + self.assertEqual(rules[0].required_hardware[0].state, LifecycleState.ACTIVE) if __name__ == '__main__': diff --git a/foreman/test/test_engine.py b/foreman/test/test_engine.py index 4aaa0c3..1b3208a 100644 --- a/foreman/test/test_engine.py +++ b/foreman/test/test_engine.py @@ -296,32 +296,40 @@ def inferred_rules_config(): ) -def test_inferred_rules_change_what_the_planner_allows(inferred_rules_config): - """Rules fed at runtime must steer planning: same state, different decision.""" +class _FakeDependencyProvider: + """Simple dependency rule provider for planner tests.""" + + def __init__(self): + self.rules = [] + + def get_dependency_rules(self): + return self.rules + + +def test_pulled_rules_change_what_the_planner_allows(inferred_rules_config): + """Planner decisions change when the provider returns different rules.""" lock = threading.Lock() engine = ForemanEngine(inferred_rules_config, lock) + provider = _FakeDependencyProvider() + engine.set_dependency_provider(provider) - # hw1 is only INACTIVE, gripper is INACTIVE, and no rules are known yet. + # hw1 is only INACTIVE, gripper is INACTIVE, and the source has no rules yet. engine.set_system_state([ Component('hw1', ComponentType.HARDWARE, LifecycleState.INACTIVE), Component('gripper', ComponentType.CONTROLLER, LifecycleState.INACTIVE), ]) engine.request_goal('run') - # No rules yet, then planner activates gripper cmd = engine.get_next_transition() assert cmd is not None assert cmd.component.name == 'gripper' assert cmd.goal_state == LifecycleState.ACTIVE - # Feed in the inferred rule "gripper needs hw1 ACTIVE" (it is only INACTIVE). - engine.set_dependency_rules([ - ControllerDependencyRule( - controller_name='gripper', - required_hardware=[HardwareRequirement('hw1', LifecycleState.ACTIVE)] - ) - ]) + # Update the provider to require hw1 ACTIVE before gripper activation. + provider.rules = [ControllerDependencyRule( + controller_name='gripper', + required_hardware=[HardwareRequirement('hw1', LifecycleState.ACTIVE)])] - # Same observed state, but now gripper is blocked and hw1 isn't in the goal -> no move. + # The newly pulled rule blocks activation because hw1 is not ACTIVE. assert engine.get_next_transition() is None diff --git a/foreman/test/test_planner.py b/foreman/test/test_planner.py index 6c3fbf6..db8ebe6 100644 --- a/foreman/test/test_planner.py +++ b/foreman/test/test_planner.py @@ -55,19 +55,20 @@ def apply_command(state: SystemState, cmd: SystemTransitionCommand): ) -def test_set_dependency_rules_swaps_whole_set(basic_planner): - """set_dependency_rules drops the old rules and installs the new ones.""" - assert 'franka_jtc' in basic_planner.rules - - basic_planner.set_dependency_rules([ - ControllerDependencyRule( - controller_name='new_ctrl', - required_hardware=[HardwareRequirement('new_hw', LifecycleState.ACTIVE)] - ) - ]) - - assert 'franka_jtc' not in basic_planner.rules - assert basic_planner.rules['new_ctrl'].required_hardware[0].name == 'new_hw' +def test_planner_pulls_rules_from_provider(basic_planner): + """Verify the planner uses rules returned by its provider.""" + class _FakeProvider: + def get_dependency_rules(self): + return [ControllerDependencyRule( + controller_name='new_ctrl', + required_hardware=[HardwareRequirement('new_hw', LifecycleState.ACTIVE)])] + + basic_planner.set_dependency_provider(_FakeProvider()) + rules = basic_planner.get_current_rules() + + # Rules now come from the provider instead of the initial configuration. + assert 'franka_jtc' not in rules + assert rules['new_ctrl'].required_hardware[0].name == 'new_hw' def test_scenario_1_standard_bring_up(basic_planner): From 1f142be494139797b046124f8c1843b17947f229 Mon Sep 17 00:00:00 2001 From: root Date: Mon, 29 Jun 2026 13:02:15 +0000 Subject: [PATCH 4/8] test(engine): cover controller with no dependency rule A goal can name a controller that has no inferred rule. The check already handles this with `if not rule: continue`; this test pins it so it can't regress into a None crash. Signed-off-by: root --- foreman/test/test_engine.py | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/foreman/test/test_engine.py b/foreman/test/test_engine.py index 1b3208a..8aa2911 100644 --- a/foreman/test/test_engine.py +++ b/foreman/test/test_engine.py @@ -333,3 +333,26 @@ def test_pulled_rules_change_what_the_planner_allows(inferred_rules_config): # The newly pulled rule blocks activation because hw1 is not ACTIVE. assert engine.get_next_transition() is None + + +def test_controller_with_no_dependency_rule_is_satisfiable(inferred_rules_config): + """A controller named in a goal but absent from the pulled rules has no + requirements, so the goal is accepted instead of crashing on a missing rule.""" + lock = threading.Lock() + engine = ForemanEngine(inferred_rules_config, lock) + + provider = _FakeDependencyProvider() + # The provider knows a rule, but for a DIFFERENT controller, so 'gripper' has no rule. + provider.rules = [ControllerDependencyRule( + controller_name='new_controller', + required_hardware=[HardwareRequirement('hw1', LifecycleState.ACTIVE)])] + engine.set_dependency_provider(provider) + + engine.set_system_state([ + Component('hw1', ComponentType.HARDWARE, LifecycleState.INACTIVE), + Component('gripper', ComponentType.CONTROLLER, LifecycleState.INACTIVE), + ]) + + # 'gripper' has no rule, so no hardware requirements and goal must be accepted, not crash. + response = engine.request_goal('run') + assert response.success is True From c09bced139224ef75fa1a0b16edcc7c33b095f29 Mon Sep 17 00:00:00 2001 From: root Date: Mon, 29 Jun 2026 13:34:21 +0000 Subject: [PATCH 5/8] refactor(types): type the dependency provider params Add a DependencyProvider Protocol in types.py and annotate the engine and planner provider parameters with it, instead of leaving them untyped. Signed-off-by: root --- foreman/foreman/engine.py | 3 ++- foreman/foreman/planner.py | 5 +++-- foreman/foreman/types.py | 8 +++++++- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/foreman/foreman/engine.py b/foreman/foreman/engine.py index bdb9a45..8107eb1 100644 --- a/foreman/foreman/engine.py +++ b/foreman/foreman/engine.py @@ -6,6 +6,7 @@ from foreman.types import Component from foreman.types import ComponentType from foreman.types import ControllerDependencyRule +from foreman.types import DependencyProvider from foreman.types import ErrorSnapshot from foreman.types import ForemanError from foreman.types import ForemanErrorCategory @@ -92,7 +93,7 @@ def abort_goal(self, error: ForemanError): self._last_issued_command = None self._locked_abort_transition() - def set_dependency_provider(self, dependency_provider): + def set_dependency_provider(self, dependency_provider: DependencyProvider): """Set the source of dependency rules for the planner.""" self._planner.set_dependency_provider(dependency_provider) diff --git a/foreman/foreman/planner.py b/foreman/foreman/planner.py index 991bddc..c5182ed 100644 --- a/foreman/foreman/planner.py +++ b/foreman/foreman/planner.py @@ -3,6 +3,7 @@ from foreman.types import Component from foreman.types import ComponentType from foreman.types import ControllerDependencyRule +from foreman.types import DependencyProvider from foreman.types import LifecycleState from foreman.types import SystemGoal from foreman.types import SystemState @@ -13,11 +14,11 @@ class Planner: """Plan the next single step towards the lifecycle state goal of the system.""" def __init__(self, dependency_rules: List[ControllerDependencyRule] = None, - dependency_provider=None): + dependency_provider: Optional[DependencyProvider] = None): self._dependency_provider = dependency_provider self.rules = {rule.controller_name: rule for rule in (dependency_rules or [])} - def set_dependency_provider(self, dependency_provider): + def set_dependency_provider(self, dependency_provider: DependencyProvider): """Set a provider for dependency rules, which can be queried at runtime.""" self._dependency_provider = dependency_provider diff --git a/foreman/foreman/types.py b/foreman/foreman/types.py index 6afb49f..118911e 100644 --- a/foreman/foreman/types.py +++ b/foreman/foreman/types.py @@ -2,7 +2,7 @@ from dataclasses import field from enum import Enum from pathlib import Path -from typing import Dict, List, Optional +from typing import Dict, List, Optional, Protocol from lifecycle_msgs.msg import State @@ -136,6 +136,12 @@ class ControllerDependencyRule: required_hardware: List[HardwareRequirement] +class DependencyProvider(Protocol): + """A source of controller dependency rules the planner can pull from.""" + + def get_dependency_rules(self) -> List['ControllerDependencyRule']: ... + + @dataclass class SystemGoal: """Named system goal state. Populated from YAML.""" From f217d4f062a25dd3acb52ce9c6dc2973a210962a Mon Sep 17 00:00:00 2001 From: root Date: Thu, 2 Jul 2026 13:48:28 +0000 Subject: [PATCH 6/8] refactor(monitor): query hardware before controllers Hardware comes up before controllers bind to it, so query hardware first. Refs: #16 Signed-off-by: root --- foreman/foreman/adapters/component_state_monitor.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/foreman/foreman/adapters/component_state_monitor.py b/foreman/foreman/adapters/component_state_monitor.py index 396989a..c7e23fc 100644 --- a/foreman/foreman/adapters/component_state_monitor.py +++ b/foreman/foreman/adapters/component_state_monitor.py @@ -1,3 +1,4 @@ +import threading from typing import Dict, List from controller_manager_msgs.msg import ControllerManagerActivity @@ -157,9 +158,9 @@ def get_dependency_rules(self): if not (self._client_list_controllers.service_is_ready() and self._client_list_hardware_components.service_is_ready()): return [] - controllers = self._client_list_controllers.call(ListControllers.Request()).controller hardware_components = self._client_list_hardware_components.call( ListHardwareComponents.Request()).component + controllers = self._client_list_controllers.call(ListControllers.Request()).controller return self.infer_dependency_rules(controllers, hardware_components) # TODO: use matched event for this topic as well? That way we know if controller manager dies. From 14f65680ea80f8ee827e528159701d3d3cd3c7d3 Mon Sep 17 00:00:00 2001 From: root Date: Thu, 2 Jul 2026 14:09:36 +0000 Subject: [PATCH 7/8] test(monitor): infer dependency rules from stored observations get_dependency_rules() will infer from the latest stored controller and hardware observations instead of blocking on the controller_manager. Refs: #16 Signed-off-by: root --- foreman/test/test_component_state_monitor.py | 37 +++++++------------- 1 file changed, 13 insertions(+), 24 deletions(-) diff --git a/foreman/test/test_component_state_monitor.py b/foreman/test/test_component_state_monitor.py index de59488..3f37544 100644 --- a/foreman/test/test_component_state_monitor.py +++ b/foreman/test/test_component_state_monitor.py @@ -82,7 +82,7 @@ def test_interface_with_no_owning_hardware_gives_no_dependency(self): class TestDependencyRulesQuery(unittest.TestCase): - """Tests querying dependency rules from the controller_manager.""" + """Tests dependency rules are inferred from the latest stored observations.""" @classmethod def setUpClass(cls): @@ -94,35 +94,24 @@ def tearDownClass(cls): def setUp(self): self.node = rclpy.create_node("test_component_state_monitor") - # ComponentStateMonitor expects these callback groups on its node; a real ForemanNode - # creates them, so we add them to this plain test node. self.node.callback_group_services = MutuallyExclusiveCallbackGroup() self.node.callback_group_subscriber = ReentrantCallbackGroup() self.addCleanup(self.node.destroy_node) self.engine = Mock() self.monitor = ComponentStateMonitor(self.node, self.engine, "test_controller_manager", []) - def test_returns_empty_when_services_not_ready(self): - # When controller_manager services are unavailable, do not attempt any queries. - with mock.patch.object(self.monitor._client_list_controllers, "call") as controllers_call: - self.assertEqual(self.monitor.get_dependency_rules(), []) - controllers_call.assert_not_called() - - def test_queries_cm_and_builds_rules(self): - controllers_response = ListControllers.Response(controller=[ - _controller_state("forward_position_controller", required_command=["joint1/position"])]) - hardware_response = ListHardwareComponents.Response(component=[ - _hardware_component("RRBot", command=["joint1/position"])]) - - with mock.patch.object(self.monitor._client_list_controllers, - "service_is_ready", return_value=True), \ - mock.patch.object(self.monitor._client_list_hardware_components, - "service_is_ready", return_value=True), \ - mock.patch.object(self.monitor._client_list_controllers, - "call", return_value=controllers_response), \ - mock.patch.object(self.monitor._client_list_hardware_components, - "call", return_value=hardware_response): - rules = self.monitor.get_dependency_rules() + def test_no_observations_yields_no_rules(self): + # Before any async refresh has stored observations, there are no rules. + self.assertEqual(self.monitor.get_dependency_rules(), []) + + def test_rules_inferred_from_latest_observations(self): + # Simulate the async refresh callbacks having stored the latest observations. + self.monitor._latest_hardware_components = [ + _hardware_component("RRBot", command=["joint1/position"])] + self.monitor._latest_controllers = [ + _controller_state("forward_position_controller", required_command=["joint1/position"])] + + rules = self.monitor.get_dependency_rules() self.assertEqual(len(rules), 1) self.assertEqual(rules[0].controller_name, "forward_position_controller") From 27fb66c617000ca447fcb826a2528dc563e3f28d Mon Sep 17 00:00:00 2001 From: root Date: Thu, 2 Jul 2026 14:33:36 +0000 Subject: [PATCH 8/8] refactor(monitor): refresh dependency observations asynchronously Replace blocking service calls in the planning path with asynchronous controller_manager queries. On each activity update, query hardware first, then controllers, and store the latest observations. get_dependency_rules() now infers dependency rules from those synchronized observations, keeping the planning loop responsive. Refs: #16 Signed-off-by: root --- .../adapters/component_state_monitor.py | 58 ++++++++++++++++--- 1 file changed, 49 insertions(+), 9 deletions(-) diff --git a/foreman/foreman/adapters/component_state_monitor.py b/foreman/foreman/adapters/component_state_monitor.py index c7e23fc..946b3fe 100644 --- a/foreman/foreman/adapters/component_state_monitor.py +++ b/foreman/foreman/adapters/component_state_monitor.py @@ -81,6 +81,12 @@ def __init__( callback_group=self._node.callback_group_subscriber ) + # Latest controller_manager observations used to infer dependency rules. + # Updated asynchronously from controller_manager callbacks. + self._dependency_lock = threading.Lock() + self._latest_controllers = [] + self._latest_hardware_components = [] + # --- Lifecycle node monitoring --- self._lc_node_get_state_clients: Dict[str, object] = {} @@ -151,17 +157,49 @@ def infer_dependency_rules(controller_states, hardware_components): def get_dependency_rules(self): """ - Query the controller_manager and build the current dependency rules. + Infer dependency rules from the latest controller_manager observations. - Returns [] until the controller_manager services are up. + The observations are refreshed asynchronously, so this method never blocks. """ - if not (self._client_list_controllers.service_is_ready() - and self._client_list_hardware_components.service_is_ready()): - return [] - hardware_components = self._client_list_hardware_components.call( - ListHardwareComponents.Request()).component - controllers = self._client_list_controllers.call(ListControllers.Request()).controller - return self.infer_dependency_rules(controllers, hardware_components) + with self._dependency_lock: + return self.infer_dependency_rules( + self._latest_controllers, self._latest_hardware_components) + + def _refresh_dependency_rules(self): + """Refresh dependency observations asynchronously. + + Query hardware first, then controllers, before rebuilding dependencies. + """ + if not (self._client_list_hardware_components.service_is_ready() + and self._client_list_controllers.service_is_ready()): + return + future = self._client_list_hardware_components.call_async( + ListHardwareComponents.Request()) + future.add_done_callback(self._on_hardware_components_response) + + def _on_hardware_components_response(self, future): + """Store the latest hardware observations and request controllers.""" + try: + hardware = future.result().component + except Exception as e: + self._node.get_logger().warning( + f"{self._logger_prefix} Failed to list hardware components: {e}") + return + with self._dependency_lock: + self._latest_hardware_components = hardware + ctrl_future = self._client_list_controllers.call_async(ListControllers.Request()) + ctrl_future.add_done_callback(self._on_controllers_response) + + def _on_controllers_response(self, future): + """Store the latest controller observations.""" + try: + controllers = future.result().controller + except Exception as e: + self._node.get_logger().warning( + f"{self._logger_prefix} Failed to list controllers: {e}") + return + with self._dependency_lock: + self._latest_controllers = controllers # TODO: use matched event for this topic as well? That way we know if controller manager dies. # Minor. Currently we catch unexpected transitions in the engine (all components go to finalized) @@ -191,6 +229,8 @@ def _activity_callback(self, msg: ControllerManagerActivity): self._cm_components = components self._push_merged_state() + # Refresh dependency observations after receiving a new activity update. + self._refresh_dependency_rules() def _on_lifecycle_publisher_matched(self, name: str, info: QoSSubscriptionMatchedInfo): """DDS matched event: lifecycle node's transition_event publisher appeared or disappeared."""