From 0df60404449e8ee925e9c95b04c4f03c402df5d3 Mon Sep 17 00:00:00 2001 From: Pim Hutting Date: Tue, 13 Jan 2026 11:24:35 +0100 Subject: [PATCH 1/4] feat: added sending goal overwrites in Userinter. ref: N25B-400 --- .../agents/user_interrupt/user_interrupt_agent.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/control_backend/agents/user_interrupt/user_interrupt_agent.py b/src/control_backend/agents/user_interrupt/user_interrupt_agent.py index 05af28a..708e3e5 100644 --- a/src/control_backend/agents/user_interrupt/user_interrupt_agent.py +++ b/src/control_backend/agents/user_interrupt/user_interrupt_agent.py @@ -118,6 +118,12 @@ class UserInterruptAgent(BaseAgent): "Forwarded button press (override) with context '%s' to BDIProgramManager.", event_context, ) + elif asl_goal := self._goal_map.get(ui_id): + await self._send_to_bdi("complete_goal", asl_goal) + self.logger.info( + "Forwarded button press (override) with context '%s' to BDI Core.", + event_context, + ) else: self.logger.warning("Could not determine which element to override.") elif event_type == "pause": From 177e844349df6b2ebebe2f4408616befcd926f2b Mon Sep 17 00:00:00 2001 From: Kasper Marinus Date: Tue, 13 Jan 2026 11:46:17 +0100 Subject: [PATCH 2/4] feat: send achieved goal from interrupt->manager->semantic ref: N25B-400 --- .../agents/bdi/bdi_program_manager.py | 54 ++++++++++++++++--- .../user_interrupt/user_interrupt_agent.py | 8 +++ 2 files changed, 54 insertions(+), 8 deletions(-) diff --git a/src/control_backend/agents/bdi/bdi_program_manager.py b/src/control_backend/agents/bdi/bdi_program_manager.py index 092a2c6..7e6dfc0 100644 --- a/src/control_backend/agents/bdi/bdi_program_manager.py +++ b/src/control_backend/agents/bdi/bdi_program_manager.py @@ -42,6 +42,16 @@ class BDIProgramManager(BaseAgent): def _initialize_internal_state(self, program: Program): self._program = program self._phase = program.phases[0] # start in first phase + self._goal_mapping: dict[str, Goal] = {} + for phase in program.phases: + for goal in phase.goals: + self._populate_goal_mapping_with_goal(goal) + + def _populate_goal_mapping_with_goal(self, goal: Goal): + self._goal_mapping[str(goal.id)] = goal + for step in goal.plan.steps: + if isinstance(step, Goal): + self._populate_goal_mapping_with_goal(step) async def _create_agentspeak_and_send_to_bdi(self, program: Program): """ @@ -73,6 +83,9 @@ class BDIProgramManager(BaseAgent): phases = json.loads(msg.body) await self._transition_phase(phases["old"], phases["new"]) + case "achieve_goal": + goal_id = msg.body + self._send_achieved_goal_to_semantic_belief_extractor(goal_id) async def _transition_phase(self, old: str, new: str): if old != str(self._phase.id): @@ -138,6 +151,19 @@ class BDIProgramManager(BaseAgent): await self.send(message) + @staticmethod + def _extract_goals_from_goal(goal: Goal) -> list[Goal]: + """ + Extract all goals from a given goal, that is: the goal itself and any subgoals. + + :return: All goals within and including the given goal. + """ + goals: list[Goal] = [goal] + for plan in goal.plan: + if isinstance(plan, Goal): + goals.extend(BDIProgramManager._extract_goals_from_goal(plan)) + return goals + def _extract_current_goals(self) -> list[Goal]: """ Extract all goals from the program, including subgoals. @@ -146,15 +172,8 @@ class BDIProgramManager(BaseAgent): """ goals: list[Goal] = [] - def extract_goals_from_goal(goal_: Goal) -> list[Goal]: - goals_: list[Goal] = [goal] - for plan in goal_.plan: - if isinstance(plan, Goal): - goals_.extend(extract_goals_from_goal(plan)) - return goals_ - for goal in self._phase.goals: - goals.extend(extract_goals_from_goal(goal)) + goals.extend(self._extract_goals_from_goal(goal)) return goals @@ -173,6 +192,25 @@ class BDIProgramManager(BaseAgent): await self.send(message) + async def _send_achieved_goal_to_semantic_belief_extractor(self, achieved_goal_id: str): + """ + Inform the semantic belief extractor when a goal is marked achieved. + + :param achieved_goal_id: The id of the achieved goal. + """ + goal = self._goal_mapping.get(achieved_goal_id) + if goal is None: + self.logger.debug(f"Goal with ID {achieved_goal_id} marked achieved but was not found.") + return + + goals = self._extract_goals_from_goal(goal) + message = InternalMessage( + to=settings.agent_settings.text_belief_extractor_name, + body=GoalList(goals=goals).model_dump_json(), + thread="achieved_goals", + ) + await self.send(message) + async def _send_clear_llm_history(self): """ Clear the LLM Agent's conversation history. diff --git a/src/control_backend/agents/user_interrupt/user_interrupt_agent.py b/src/control_backend/agents/user_interrupt/user_interrupt_agent.py index 708e3e5..d92a071 100644 --- a/src/control_backend/agents/user_interrupt/user_interrupt_agent.py +++ b/src/control_backend/agents/user_interrupt/user_interrupt_agent.py @@ -124,6 +124,14 @@ class UserInterruptAgent(BaseAgent): "Forwarded button press (override) with context '%s' to BDI Core.", event_context, ) + + goal_achieve_msg = InternalMessage( + to=settings.agent_settings.bdi_program_manager_name, + thread="achieve_goal", + body=ui_id, + ) + + await self.send(goal_achieve_msg) else: self.logger.warning("Could not determine which element to override.") elif event_type == "pause": From 65e0b2d250cd4a68fe0b5e2b8e7a9fadf3f150c4 Mon Sep 17 00:00:00 2001 From: Pim Hutting Date: Tue, 13 Jan 2026 12:05:20 +0100 Subject: [PATCH 3/4] feat: added correct message ref: N25B-400 --- .../user_interrupt/user_interrupt_agent.py | 20 ++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/control_backend/agents/user_interrupt/user_interrupt_agent.py b/src/control_backend/agents/user_interrupt/user_interrupt_agent.py index d92a071..58d2024 100644 --- a/src/control_backend/agents/user_interrupt/user_interrupt_agent.py +++ b/src/control_backend/agents/user_interrupt/user_interrupt_agent.py @@ -7,6 +7,7 @@ from control_backend.agents import BaseAgent from control_backend.agents.bdi.agentspeak_generator import AgentSpeakGenerator from control_backend.core.agent_system import InternalMessage from control_backend.core.config import settings +from control_backend.schemas.belief_message import Belief, BeliefMessage from control_backend.schemas.program import ConditionalNorm, Program from control_backend.schemas.ri_message import ( GestureCommand, @@ -119,7 +120,7 @@ class UserInterruptAgent(BaseAgent): event_context, ) elif asl_goal := self._goal_map.get(ui_id): - await self._send_to_bdi("complete_goal", asl_goal) + await self._send_to_bdi_belief(asl_goal) self.logger.info( "Forwarded button press (override) with context '%s' to BDI Core.", event_context, @@ -134,6 +135,9 @@ class UserInterruptAgent(BaseAgent): await self.send(goal_achieve_msg) else: self.logger.warning("Could not determine which element to override.") + self.logger.warning(self._goal_map) + self.loger.warning(ui_id) + elif event_type == "pause": self.logger.debug( "Received pause/resume button press with context '%s'.", event_context @@ -305,6 +309,20 @@ class UserInterruptAgent(BaseAgent): await self.send(msg) self.logger.info(f"Directly forced {thread} in BDI: {body}") + async def _send_to_bdi_belief(self, asl_goal: str): + """Send belief to BDI Core""" + belief_name = f"achieved_{asl_goal}" + belief = Belief(name=belief_name) + self.logger.debug(f"Sending belief to BDI Core: {belief_name}") + belief_message = BeliefMessage(create=[belief]) + msg = InternalMessage( + to=settings.agent_settings.bdi_core_name, + thread="belief_update", + body=belief_message.model_dump_json(), + ) + await self.send(msg) + self.logger.info(f"Sent belief to BDI Core: {msg}") + async def _send_experiment_control_to_bdi_core(self, type): """ method to send experiment control buttons to bdi core. From f87651f691f0b813122decf6725d264623fb13d4 Mon Sep 17 00:00:00 2001 From: Kasper Marinus Date: Tue, 13 Jan 2026 12:26:18 +0100 Subject: [PATCH 4/4] fix: achieved goal in bdi core ref: N25B-400 --- src/control_backend/agents/bdi/bdi_program_manager.py | 2 +- .../agents/user_interrupt/user_interrupt_agent.py | 7 ++----- src/control_backend/api/v1/endpoints/robot.py | 1 - src/control_backend/schemas/belief_message.py | 2 +- 4 files changed, 4 insertions(+), 8 deletions(-) diff --git a/src/control_backend/agents/bdi/bdi_program_manager.py b/src/control_backend/agents/bdi/bdi_program_manager.py index 7e6dfc0..25b7364 100644 --- a/src/control_backend/agents/bdi/bdi_program_manager.py +++ b/src/control_backend/agents/bdi/bdi_program_manager.py @@ -85,7 +85,7 @@ class BDIProgramManager(BaseAgent): await self._transition_phase(phases["old"], phases["new"]) case "achieve_goal": goal_id = msg.body - self._send_achieved_goal_to_semantic_belief_extractor(goal_id) + await self._send_achieved_goal_to_semantic_belief_extractor(goal_id) async def _transition_phase(self, old: str, new: str): if old != str(self._phase.id): diff --git a/src/control_backend/agents/user_interrupt/user_interrupt_agent.py b/src/control_backend/agents/user_interrupt/user_interrupt_agent.py index 58d2024..4bf681a 100644 --- a/src/control_backend/agents/user_interrupt/user_interrupt_agent.py +++ b/src/control_backend/agents/user_interrupt/user_interrupt_agent.py @@ -32,7 +32,7 @@ class UserInterruptAgent(BaseAgent): Prioritized actions clear the current RI queue before inserting the new item, ensuring they are executed immediately after Pepper's current action has been fulfilled. - :ivar sub_socket: The ZMQ SUB socket used to receive user intterupts. + :ivar sub_socket: The ZMQ SUB socket used to receive user interrupts. """ def __init__(self, **kwargs): @@ -135,8 +135,6 @@ class UserInterruptAgent(BaseAgent): await self.send(goal_achieve_msg) else: self.logger.warning("Could not determine which element to override.") - self.logger.warning(self._goal_map) - self.loger.warning(ui_id) elif event_type == "pause": self.logger.debug( @@ -317,11 +315,10 @@ class UserInterruptAgent(BaseAgent): belief_message = BeliefMessage(create=[belief]) msg = InternalMessage( to=settings.agent_settings.bdi_core_name, - thread="belief_update", + thread="beliefs", body=belief_message.model_dump_json(), ) await self.send(msg) - self.logger.info(f"Sent belief to BDI Core: {msg}") async def _send_experiment_control_to_bdi_core(self, type): """ diff --git a/src/control_backend/api/v1/endpoints/robot.py b/src/control_backend/api/v1/endpoints/robot.py index afbf1ac..95a9c40 100644 --- a/src/control_backend/api/v1/endpoints/robot.py +++ b/src/control_backend/api/v1/endpoints/robot.py @@ -137,7 +137,6 @@ async def ping_stream(request: Request): logger.info("Client disconnected from SSE") break - logger.debug(f"Yielded new connection event in robot ping router: {str(connected)}") connectedJson = json.dumps(connected) yield (f"data: {connectedJson}\n\n") diff --git a/src/control_backend/schemas/belief_message.py b/src/control_backend/schemas/belief_message.py index 51411b3..226833e 100644 --- a/src/control_backend/schemas/belief_message.py +++ b/src/control_backend/schemas/belief_message.py @@ -11,7 +11,7 @@ class Belief(BaseModel): """ name: str - arguments: list[str] | None + arguments: list[str] | None = None # To make it hashable model_config = {"frozen": True}