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():