From bf49376d3c7f36b46c4d080b8389e2115e19c9fc Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Mon, 18 Mar 2024 18:25:58 +0100
Subject: [PATCH] Adding Multithreading (as Callback Group) to have the
 task_execute_callback next to the State MAchine

---
 .../interaction_tasks/task_node.py            | 20 ++++++++++++++++---
 1 file changed, 17 insertions(+), 3 deletions(-)

diff --git a/src/interaction_tasks/interaction_tasks/task_node.py b/src/interaction_tasks/interaction_tasks/task_node.py
index 0773d1e..8d623c8 100644
--- a/src/interaction_tasks/interaction_tasks/task_node.py
+++ b/src/interaction_tasks/interaction_tasks/task_node.py
@@ -3,6 +3,7 @@ from rclpy.node import Node
 from rclpy.parameter import Parameter
 from rclpy.action import ActionServer
 from rclpy.action import GoalResponse, CancelResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
 
 from transitions import Machine
 import yaml
@@ -22,6 +23,7 @@ class TaskNode(Node):
         self.number_of_processed_trajectories = 0
         self.goal_dict = {}
         # ROS2 Interfaces
+        self.task_callback_group = ReentrantCallbackGroup()
 
         # Topic
 
@@ -34,7 +36,8 @@ class TaskNode(Node):
                                          'interaction/task_action',
                                          goal_callback=self._task_goal_callback,
                                          cancel_callback=self._task_cancel_callback,
-                                         execute_callback=self._task_execute_callback)
+                                         execute_callback=self._task_execute_callback,
+                                         callback_group=self.task_callback_group)
 
         # State Machine
         self.state = None
@@ -42,7 +45,7 @@ class TaskNode(Node):
         self.states = [
             'waiting_for_task_specs',
             'processing_non_interactive_input',
-            'processing_interactive_inpute',
+            'processing_interactive_input',
             'waiting_for_robot_response',
             'waiting_for_objective_function_response',
             'sending_request'
@@ -69,6 +72,18 @@ class TaskNode(Node):
         super().destroy_node()
 
     # State Methods
+    def on_enter_processing_non_interactive_input(self):
+        pass
+
+    def on_enter_processing_interactive_input(self):
+        pass
+
+    def on_enter_waiting_for_robot_response(self):
+        pass
+
+    def on_enter_waiting_for_objective_function_response(self):
+        pass
+
 
     # Callback functions
     def _task_goal_callback(self, goal):
@@ -99,7 +114,6 @@ class TaskNode(Node):
             if self.state == 'sending_request':
                 result_msg.score = self.goal_dict['score']
                 result_msg.new_means = self.goal_dict['new_means']
-
                 break
 
         if goal_handle.is_cancel_requested():