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.parameter import Parameter
from rclpy.action import ActionServer from rclpy.action import ActionServer
from rclpy.action import GoalResponse, CancelResponse from rclpy.action import GoalResponse, CancelResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from transitions import Machine from transitions import Machine
import yaml import yaml
@ -22,6 +23,7 @@ class TaskNode(Node):
self.number_of_processed_trajectories = 0 self.number_of_processed_trajectories = 0
self.goal_dict = {} self.goal_dict = {}
# ROS2 Interfaces # ROS2 Interfaces
self.task_callback_group = ReentrantCallbackGroup()
# Topic # Topic
@ -34,7 +36,8 @@ class TaskNode(Node):
'interaction/task_action', 'interaction/task_action',
goal_callback=self._task_goal_callback, goal_callback=self._task_goal_callback,
cancel_callback=self._task_cancel_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 # State Machine
self.state = None self.state = None
@ -42,7 +45,7 @@ class TaskNode(Node):
self.states = [ self.states = [
'waiting_for_task_specs', 'waiting_for_task_specs',
'processing_non_interactive_input', 'processing_non_interactive_input',
'processing_interactive_inpute', 'processing_interactive_input',
'waiting_for_robot_response', 'waiting_for_robot_response',
'waiting_for_objective_function_response', 'waiting_for_objective_function_response',
'sending_request' 'sending_request'
@ -69,6 +72,18 @@ class TaskNode(Node):
super().destroy_node() super().destroy_node()
# State Methods # 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 # Callback functions
def _task_goal_callback(self, goal): def _task_goal_callback(self, goal):
@ -99,7 +114,6 @@ class TaskNode(Node):
if self.state == 'sending_request': if self.state == 'sending_request':
result_msg.score = self.goal_dict['score'] result_msg.score = self.goal_dict['score']
result_msg.new_means = self.goal_dict['new_means'] result_msg.new_means = self.goal_dict['new_means']
break break
if goal_handle.is_cancel_requested(): if goal_handle.is_cancel_requested():