Niko/InteractiveRobotLearning
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Adding Multithreading (as Callback Group) to have the task_execute_callback next to the State MAchine

Browse Source
This commit is contained in:
Niko Feith 2024-03-18 18:25:58 +01:00
parent f875ee34c1
commit bf49376d3c
1 changed files with 17 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/interaction_tasks/interaction_tasks/task_node.py
View File

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