feat: Serve brakfast task manager without nav

README.md

@@ -76,6 +76,14 @@ Additional commands can be added within the Makefile and the scripts inside the
 
 The `frida_task_manager` is the central package in charge of processing the command information and directing the tasks with its respective area. For code readability, each area development its inside a Python module `area_tasks.py`, and they are called by `task_manager_server.py`, the script were the ROS node is created.
 
+At the top of this mentioned file, there are four constants to enable or disable the tasks, you can change any of these to `False` is for the current test there is no need of an area:
+```python
+NAV_ENABLED = True 
+MANIPULATION_ENABLED = True
+CONVERSATION_ENABLED = True 
+VISION_ENABLED = False
+```
+
 For executing the `task_manager`, follow the above steps of [Docker setup](#Docker%20Development). Inside the `bash` terminal, setup the ROS network and execute the node:
 ```bash
 export ROS_MASTER_URI=http://192.168.31.23:11311 # IP of the ROS Master (if master is in another machine)

ws/src/frida_task_manager/scripts/breakfast_task_manager.py

@@ -0,0 +1,168 @@
+#!/usr/bin/env python3
+
+"""
+Task manager for the Breakfast task of RoboCup @Home 2024
+"""
+
+### Import libraries
+import rospy
+import actionlib
+
+### ROS messages
+from std_msgs.msg import String
+from frida_hri_interfaces.msg import Command, CommandList
+from frida_hri_interfaces.msg import ConversateAction, ConversateFeedback, ConversateGoal, ConversateResult
+
+### Python submodules
+from hri_tasks import TasksHRI
+from manipulation_tasks import TasksManipulation
+from nav_tasks import TasksNav
+
+COMMANDS_TOPIC = "/task_manager/commands"
+SPEAK_TOPIC = "/speech/speak"
+CONVERSATION_SERVER = "/conversation_as"
+
+NAV_ENABLED = True
+MANIPULATION_ENABLED = True
+CONVERSATION_ENABLED = False
+VISION_ENABLED = False
+
+AREAS = ["nav", "manipulation", "hri", "vision"]
+
+AREA_ENABLED = {
+    "nav": NAV_ENABLED,
+    "manipulation": MANIPULATION_ENABLED,
+    "hri": CONVERSATION_ENABLED,
+    "vision": VISION_ENABLED
+}
+
+class TaskManagerServer:
+    """Class to manage different tasks divided by categories"""
+    STATE_ENUM = {
+        "IDLE": 0,
+        "RECEIVE_COMMANDS": 1,
+        "EXECUTING_COMMANDS": 2,
+        "STOPPING": 3,
+        "ERROR": 4,
+        "SHUTDOWN": 5
+    }
+
+    COMMANDS_CATEGORY = {
+        "nav" : ["go", "follow", "stop", "approach", "remember"],
+        "manipulation" : ["pick", "place", "grasp", "give", "open", "close", "pour"],
+        "hri" : ["ask", "interact", "feedback"],
+        "vision" : ["find", "identify", "count"]
+    }
+
+    def __init__(self) -> None:
+        self._node = rospy.init_node("task_manager_server")
+        self._rate = rospy.Rate(200)
+        self._sub = rospy.Subscriber(COMMANDS_TOPIC, CommandList, self.commands_callback)
+
+        # Creates an empty dictionary to store the subtask manager of each area
+        self.subtask_manager = dict.fromkeys(AREAS, None)
+
+        if CONVERSATION_ENABLED:
+            self.subtask_manager["hri"] = TasksHRI()
+            self.subtask_manager["hri"].speak("Hi, my name is Frida. I'm here to help you with your domestic tasks")
+        if MANIPULATION_ENABLED:
+            self.subtask_manager["manipulation"] = TasksManipulation()
+        if NAV_ENABLED:
+            self.subtask_manager["nav"] = TasksNav()
+        #if VISION_ENABLED:
+            #self.subtask_manager["vision"] = TasksVision()
+
+        self.current_state = TaskManagerServer.STATE_ENUM["IDLE"]
+        self.current_past_state = None
+        self.current_command = None
+        self.current_queue = []
+        self.perceived_information = ""
+
+        self.current_queue = [
+            Command(action="pick", complement="zucaritas"),
+            Command(action="pour", complement="zucaritas"),
+            Command(action="place", complement="zucaritas"),
+            Command(action="pick", complement="cocacola"),
+            Command(action="pour", complement="cocacola"),
+            Command(action="place", complement="cocacola")
+        ]
+
+        self.run()
+
+    def commands_callback(self, commands_input: CommandList) -> None:
+        """Receive processed commands from the interpreter and call executions from the queue"""
+        rospy.loginfo("Received commands")
+
+        if not commands_input:
+            self.current_state = TaskManagerServer.STATE_ENUM["ERROR"]
+            return
+
+        if self.current_thread is not None:
+            return
+
+        if self.current_state != TaskManagerServer.STATE_ENUM["IDLE"]:
+            rospy.logerr("Cancelling current commands and executing new received ")
+            self.current_queue = []
+            self.cancel_command()
+            self.current_state = TaskManagerServer.STATE_ENUM["IDLE"]
+
+        self.current_state = TaskManagerServer.STATE_ENUM["RECEIVE_COMMANDS"]
+        self.current_queue = commands_input.commands
+        self.past_state = self.current_state
+
+        #self.say("I 'have finished my tasks, I'm going to rest now")
+
+    def execute_command(self, command: Command) -> int:
+        """Method for executing a single command inside its area submodule"""
+
+        rospy.loginfo(f"Executing command: {command.action} -> {command.complement}")
+
+        task_result = 0
+        for area in AREAS:
+            if command.action in TaskManagerServer.COMMANDS_CATEGORY[area] and AREA_ENABLED[area]:
+                task_result = self.subtask_manager[area].execute_command(
+                    command.action, command.complement, self.perceived_information
+                )
+
+        if task_result == -1:
+            rospy.logerr("Error in task execution")
+            return TaskManagerServer.STATE_ENUM["ERROR"]
+
+        self.perceived_information += f"{command.action} {command.complement} {task_result} "
+        return TaskManagerServer.STATE_ENUM["EXECUTING_COMMANDS"]
+
+    def cancel_command(self) -> None:
+        """Method to cancel the current command"""
+        for area in AREAS:
+            if self.current_command in TaskManagerServer.COMMANDS_CATEGORY[area]:
+                self.subtask_manager[area].cancel_command()
+
+    def run(self) -> None:
+        """Main loop for the task manager"""
+        while not rospy.is_shutdown():
+
+            if len(self.current_queue) > 0: #and self.current_thread == current_thread:
+                self.current_command = self.current_queue.pop(0)
+                self.status = self.execute_command(self.current_command)
+                if self.status == TaskManagerServer.STATE_ENUM["ERROR"]:
+                    rospy.logerr("Error in command manager")
+                    self.current_state = TaskManagerServer.STATE_ENUM["ERROR"]
+                    self.cancel_command()
+                    self.current_queue = []
+                    self.state = TaskManagerServer.STATE_ENUM["IDLE"]
+                    self.past_state = self.status
+                    continue
+            elif self.current_state != TaskManagerServer.STATE_ENUM["IDLE"]:
+                self.current_thread = None
+                self.current_queue = []
+                self.current_state = TaskManagerServer.STATE_ENUM["IDLE"]
+
+                #self.subtask_manager["hri"].speak("I have finished my tasks, I'm going to rest now")
+            self._rate.sleep()
+
+
+if __name__ == "__main__":
+    try:
+        TaskManagerServer()
+    except rospy.ROSInterruptException as e:
+        rospy.logerr(f'Error: {e}')