From 28f1a0a931d14465b1eb18e235b2c7df809e2043 Mon Sep 17 00:00:00 2001 From: Patricio Loncomilla Date: Tue, 4 Nov 2025 18:37:57 -0300 Subject: [PATCH] fixed state_logger and be_action_server for ros2 --- flexbe_core/flexbe_core/state_logger.py | 7 ++- flexbe_widget/bin/be_action_server | 7 ++- .../flexbe_widget/behavior_action_server.py | 55 ++++++++++++++++--- 3 files changed, 60 insertions(+), 9 deletions(-) diff --git a/flexbe_core/flexbe_core/state_logger.py b/flexbe_core/flexbe_core/state_logger.py index c3491e8..ad818f1 100644 --- a/flexbe_core/flexbe_core/state_logger.py +++ b/flexbe_core/flexbe_core/state_logger.py @@ -110,6 +110,11 @@ def initialize(be_name=None): StateLogger._serialize_impl = StateLogger._log_serialize.get_parameter_value().string_value + if StateLogger._log_config.get_parameter_value().string_value != "": + yaml_extra_params = yaml.safe_load(StateLogger._log_config.get_parameter_value().string_value) + else: + yaml_extra_params = {} + logger_config = dict({ 'version': 1, 'disable_existing_loggers': False, @@ -127,7 +132,7 @@ def initialize(be_name=None): } }, 'loggers': {'flexbe': {'level': 'INFO', 'handlers': ['file']}} - }, **yaml.safe_load(StateLogger._log_config.get_parameter_value().string_value)) + }, **yaml_extra_params) if ('handlers' in logger_config and 'file' in logger_config['handlers'] and 'filename' in logger_config['handlers']['file']): diff --git a/flexbe_widget/bin/be_action_server b/flexbe_widget/bin/be_action_server index 27be47f..b01836d 100755 --- a/flexbe_widget/bin/be_action_server +++ b/flexbe_widget/bin/be_action_server @@ -8,15 +8,20 @@ fi # flake8: noqa import rclpy from flexbe_widget.behavior_action_server import BehaviorActionServer +from rclpy.executors import MultiThreadedExecutor def main(args=None): rclpy.init(args=args) + executor = MultiThreadedExecutor() + node = rclpy.create_node('flexbe_action_server') server = BehaviorActionServer(node) + executor.add_node(node) # Wait for ctrl-c to stop the application - rclpy.spin(node) + executor.spin() + node.destroy_node() rclpy.shutdown() if __name__ == '__main__': diff --git a/flexbe_widget/flexbe_widget/behavior_action_server.py b/flexbe_widget/flexbe_widget/behavior_action_server.py index c220adb..15c0948 100644 --- a/flexbe_widget/flexbe_widget/behavior_action_server.py +++ b/flexbe_widget/flexbe_widget/behavior_action_server.py @@ -42,6 +42,7 @@ from flexbe_msgs.msg import BEStatus, BehaviorModification, BehaviorSelection from rclpy.action import ActionServer +from rclpy.action.server import ServerGoalHandle from rosidl_runtime_py import get_interface_path @@ -49,6 +50,9 @@ import yaml +import rclpy +import time +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup class BehaviorActionServer: """Behavior action server.""" @@ -62,24 +66,31 @@ def __init__(self, node): self._current_state = None self._active_behavior_id = None + self.topic_group = MutuallyExclusiveCallbackGroup() + self.action_group = MutuallyExclusiveCallbackGroup() + self._pub = self._node.create_publisher(BehaviorSelection, Topics._START_BEHAVIOR_TOPIC, 100) self._preempt_pub = self._node.create_publisher(Empty, Topics._CMD_PREEMPT_TOPIC, 100) - self._status_pub = self._node.create_subscription(BEStatus, Topics._ONBOARD_STATUS_TOPIC, self._status_cb, 100) - self._state_pub = self._node.create_subscription(Int32, Topics._BEHAVIOR_UPDATE_TOPIC, self._state_cb, 100) + + self._status_pub = self._node.create_subscription(BEStatus, Topics._ONBOARD_STATUS_TOPIC, self._status_cb, 100, callback_group = self.topic_group) + self._state_pub = self._node.create_subscription(Int32, Topics._BEHAVIOR_UPDATE_TOPIC, self._state_cb, 100, callback_group = self.topic_group) self._as = ActionServer(self._node, BehaviorExecution, Topics._EXECUTE_BEHAVIOR_ACTION, - goal_callback=self._goal_cb, + handle_accepted_callback=self._goal_cb, cancel_callback=self._preempt_cb, - execute_callback=self._execute_cb) + execute_callback=self._execute_cb, + callback_group=self.action_group) self._behavior_lib = BehaviorLibrary(node) self._node.get_logger().info('%d behaviors available, ready for start request.' % self._behavior_lib.count_behaviors()) + self.running = False - def _goal_cb(self, goal_handle): + + def _goal_cb(self, goal_handle : ServerGoalHandle): self._current_goal = goal_handle - goal = goal_handle.request() + goal = goal_handle.request if self._preempt_requested: goal_handle.canceled() @@ -152,24 +163,45 @@ def _goal_cb(self, goal_handle): self._current_state = None self._behavior_started = False self._preempt_requested = False + self.running = True + self.outcome = '' # start new behavior self._pub.publish(be_selection) + self._current_goal.execute() def _preempt_cb(self, goal_handle): self._preempt_requested = True if not self._behavior_started: return + # Send the preempt request to real behavior self._preempt_pub.publish(Empty()) self._node.get_logger().info('Behavior execution preempt requested!') + def clean_me(self): + self.running = False + self._current_state = None + self._behavior_started = False + self._preempt_requested = False + def _execute_cb(self, goal_handle): self._node.get_logger().info('Executing behavior') + while rclpy.ok() and self.running: + time.sleep(0.01) + print("End execution") + self.clean_me() + result = BehaviorExecution.Result() + result.outcome = self.outcome + self.outcome = "" + return result + def _status_cb(self, msg): if msg.code == BEStatus.ERROR: self._node.get_logger().error('Failed to run behavior! Check onboard terminal for further infos.') self._current_goal.abort() + self.outcome = "error" + self.clean_me() return if not self._behavior_started and msg.code == BEStatus.STARTED: @@ -191,10 +223,19 @@ def _status_cb(self, msg): elif msg.code == BEStatus.FINISHED: result = msg.args[0] if len(msg.args) >= 1 else '' self._node.get_logger().info("Finished behavior execution with result '%s'!" % result) - self._current_goal.succeed() + if result == 'preempted': + self._current_goal.succeed() # .canceled() + self.outcome = "preempted" + self.clean_me() + else: + self._current_goal.succeed() + self.outcome = "success" + self.clean_me() elif msg.code == BEStatus.FAILED: self._node.get_logger().error("Behavior execution failed in state '%s'!" % str(self._current_state)) self._current_goal.abort() + self.outcome = "failed" + self.clean_me() def _state_cb(self, msg): self._current_state = msg.data