Merge branch 'main' of https://git.science.uu.nl/ics/sp/2025/n25b/pepperplus-cb into feat/face-recognition

src/control_backend/agents/user_interrupt/user_interrupt_agent.py

@@ -1,4 +1,5 @@
 import json
+import logging
 
 import zmq
 from zmq.asyncio import Context
@@ -8,13 +9,15 @@ 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.program import ConditionalNorm, Goal, Program
 from control_backend.schemas.ri_message import (
     GestureCommand,
     RIEndpoint,
     SpeechCommand,
 )
 
+experiment_logger = logging.getLogger(settings.logging_settings.experiment_logger_name)
+
 
 class UserInterruptAgent(BaseAgent):
     """
@@ -245,6 +248,16 @@ class UserInterruptAgent(BaseAgent):
             self._cond_norm_map = {}
             self._cond_norm_reverse_map = {}
 
+            def _register_goal(goal: Goal):
+                """Recursively register goals and their subgoals."""
+                slug = AgentSpeakGenerator.slugify(goal)
+                self._goal_map[str(goal.id)] = slug
+                self._goal_reverse_map[slug] = str(goal.id)
+
+                for step in goal.plan.steps:
+                    if isinstance(step, Goal):
+                        _register_goal(step)
+
             for phase in program.phases:
                 for trigger in phase.triggers:
                     slug = AgentSpeakGenerator.slugify(trigger)
@@ -252,8 +265,7 @@ class UserInterruptAgent(BaseAgent):
                     self._trigger_reverse_map[slug] = str(trigger.id)
 
                 for goal in phase.goals:
-                    self._goal_map[str(goal.id)] = AgentSpeakGenerator.slugify(goal)
-                    self._goal_reverse_map[AgentSpeakGenerator.slugify(goal)] = str(goal.id)
+                    _register_goal(goal)
 
                 for goal, id in self._goal_reverse_map.items():
                     self.logger.debug(f"Goal mapping: UI ID {goal} -> {id}")
@@ -295,6 +307,7 @@ class UserInterruptAgent(BaseAgent):
 
         :param text_to_say: The string that the robot has to say.
         """
+        experiment_logger.chat(text_to_say, extra={"role": "assistant"})
         cmd = SpeechCommand(data=text_to_say, is_priority=True)
         out_msg = InternalMessage(
             to=settings.agent_settings.robot_speech_name,
@@ -334,6 +347,7 @@ class UserInterruptAgent(BaseAgent):
             belief_name = f"force_{asl}"
         else:
             self.logger.warning("Tried to send belief with unknown type")
+            return
         belief = Belief(name=belief_name, arguments=None)
         self.logger.debug(f"Sending belief to BDI Core: {belief_name}")
         # Conditional norms are unachieved by removing the belief