diff --git a/.github/workflows/flexbe_ci.yml b/.github/workflows/flexbe_ci.yml index 12e4236..a7f3ba4 100644 --- a/.github/workflows/flexbe_ci.yml +++ b/.github/workflows/flexbe_ci.yml @@ -10,6 +10,10 @@ jobs: matrix: ros: [jazzy] include: + # - os: ubuntu-24.04 + # ros: rolling + # python: python3 + # ci_branch: ros2-devel - os: ubuntu-24.04 ros: jazzy python: python3 diff --git a/README.md b/README.md index 3c3a6e3..666eb12 100644 --- a/README.md +++ b/README.md @@ -3,8 +3,8 @@ FlexBE is a high-level behavior engine coordinating the capabilities of a robot in order to solve complex tasks. Behaviors are modeled as hierarchical state machines (HFSM) where states correspond to active actions and transitions describe the reaction to outcomes. -Main advantage over similar approaches is the good operator integration and an -intuitive user interface. + +The main advantage of FlexBE over similar approaches is the good operator integration and an intuitive user interface. Besides executing behaviors in full autonomy, the operator can restrict execution of certain transitions or trigger them manually. Furthermore, FlexBE supports modifying the whole structure of a behavior during its execution without restarting it. The user interface features a runtime control interface as well as a graphical editor for state machines. @@ -17,48 +17,78 @@ You may also want to check out the quick start tutorial demonstrations at [FlexB Jazzy ![ROS Build Farm](https://build.ros2.org/job/Jdev__flexbe_behavior_engine__ubuntu_noble_amd64/badge/icon) +> Note: This version 4+ breaks compatibility with the FlexBE App. You must use the FlexBE WebUI [flexbe_webui](https://github.com/FlexBE/flexbe_webui.git) now. + + ## Installation -For released versions, FlexBE is available as ` apt install` package `ros--flexbe-*` +For released versions, FlexBE is available as `apt install` package `ros--flexbe-*` To build from source, execute the following commands to install FlexBE for ROS 2 systems: - cd "ros2_ws"/src - git clone https://github.com/FlexBE/flexbe_behavior_engine.git + `cd "ros2_ws"/src` + + `git clone https://github.com/FlexBE/flexbe_behavior_engine.git` Next, navigate to the "ros2_ws" top-level directory and build FlexBE: - colcon build + `colcon build` + +## Creating new FlexBE Behavior packages + +To begin, create your own repository for behavior development in the `${WORKSPACE_ROOT}/src` folder: + + `ros2 run flexbe_widget create_repo [your_project_name] <--non-interactive>` + +This will clone a project template (requires internet access) that contains examples and proper package definitions, +and create the ROS 2 package structure and three subfolders. -Furthermore, create your own repository for behavior development (contains examples): +For example, running - ros2 run flexbe_widget create_repo [your_project_name] + `ros2 run flexbe_widget create_repo my_project my_flexbe_project` -This version of the flexbe_behavior_engine requires version 4.0+ of the FlexBE user interface. +from the `${WORKSPACE_ROOT}/src` folder will create: + - `${WORKSPACE_ROOT}/src/my_flexbe_project` + - `${WORKSPACE_ROOT}/src/my_flexbe_project/my_flexbe_project` - the ROS meta package + - `${WORKSPACE_ROOT}/src/my_flexbe_project/my_project_flexbe_behaviors` + - `${WORKSPACE_ROOT}/src/my_flexbe_project/my_project_flexbe_states` -It is recommended to install the FlexBE user interface by following one of these steps: - * https://github.com/FlexBE/flexbe_app.git - classic FlexBE App (iron or ros2-devel branches) - * https://github.com/FlexBE/flexbe_webui.git - new Python-based webserver version +These are intended to contain your custom FlexBE state implementations and HFSM-based behaviors. + +This release of the FlexBE Behavior Engine requires version 4.1+ of the FlexBE UI. +This breaks compatibility with the older FlexBE App and now requires use of the FlexBE WebUI tool. + +It is recommended to install the FlexBE WebUI user interface: + + [FlexBE WebUI](https://github.com/FlexBE/flexbe_webui.git) - Python-based webserver version ## Usage Use the following launch file for running the onboard engine: - ros2 launch flexbe_onboard behavior_onboard.launch.py + `ros2 launch flexbe_onboard behavior_onboard.launch.py` Use the following launch file for running the operator control station (requires the FlexBE App or WebUI): - ros2 launch flexbe_webui flexbe_ocs.launch.py + `ros2 launch flexbe_webui flexbe_ocs.launch.py` + +During testing is is recommended to start the base nodes and the UI client separately: - > Note: replace `flexbe_webui` with `flexbe_app` to run the "classic" UI (after `ros2 run flexbe_app nwjs_install`). + `ros2 launch flexbe_webui flexbe_ocs.launch.py headless:=True` -Use the following launch file to run both of the above, e.g., for testing on a single computer: + `ros2 run flexbe_webui webui_client` - ros2 launch flexbe_webui flexbe_full.launch.py + See the `flexbe_webui` README for more details. + + +Use the following launch file to run the entire FlexBE system, both onboard and OCS, e.g., for testing on a single computer: + + `ros2 launch flexbe_webui flexbe_full.launch.py` For running tests use: -`colcon test --ctest-args --packages-select ` + + `colcon test --ctest-args --packages-select ` ## Next Steps @@ -94,4 +124,4 @@ Let us know if you know a paper which should be added to the list. ## Maintainers - Philipp Schillinger ([@pschillinger](https://github.com/pschillinger), [Contact](http://philserver.bplaced.net/fbe/contact.php)) -- David Conner ([@dcconner](https://github.com/dcconner)) +- David Conner ([@dcconner](https://github.com/dcconner)) [Contact](https://flexbe.readthedocs.io/en/latest/contactinfo.html) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 4724867..17da07b 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,9 +2,19 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -3.0.6 (2024-08-05) +4.0.1 (2024-09-26) +------------------ + +4.0.0 (2024-08-24) ------------------ +* Version 4.0.0 release using state_id for communication +* this breaks API with flexbe_app and requires version 4.1.0+ of the FlexBE WebUI API +3.0.7 (2024-08-24) +------------------ + +3.0.6 (2024-08-05) +------------------ 3.0.5 (2024-07-02) ------------------ diff --git a/flexbe_behavior_engine/package.xml b/flexbe_behavior_engine/package.xml index 0d7842e..eb3c2ab 100644 --- a/flexbe_behavior_engine/package.xml +++ b/flexbe_behavior_engine/package.xml @@ -1,7 +1,7 @@ flexbe_behavior_engine - 3.0.6 + 4.0.1 A meta-package to aggregate all the FlexBE packages diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index c7f1003..0b5f956 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.0.1 (2024-09-26) +------------------ +* codespell clean up +* exception inside concurrency will get re-thrown to preempt behavior +* report all states in deep_states including containers and finished states +* re-request outcome on sync request +* notify skipped for containers +* remove some stray spam; +* use jazzy test and increase some loop timings in tests due to intermittent test failure + +4.0.0 (2024-08-24) +------------------ +* Version 4.0.0 release using state_id for communication +* this breaks API with flexbe_app and requires version 4.1.0+ of the FlexBE WebUI API +* use state id consistently to avoid long path strings +* modify preempt and published outcome to improve sync +* add flexbe_outcome_listener node for simple monitoring + +3.0.7 (2024-08-24) +------------------ +* reduce default wait durations on proxy start ups; reduce start up spam +* clarify state map message +* allow controllable OSM and concurrency outputs; improve sync handling; unhandled state exception stops behavior +* modify clear action handling; retain action result status; reduce startup spam +* add initialize_flexbe_core for common initialization +* updates to ConcurrencyContainer and StateMachine to handle sync and forced outcomes + 3.0.6 (2024-08-05) ------------------ * update cancel for action client @@ -319,7 +346,7 @@ Changelog for package flexbe_core * [flexbe_core] Added priority container * [flexbe_core] Added some more documentation * [flexbe_core] Fixed initialization of input userdata in inner statemachines -* [flexbe_core] Correctly preempt auxilliary control flows in concurrency container +* [flexbe_core] Correctly preempt auxiliary control flows in concurrency container * [flexbe_core] Fixed a bug with concurrent execution: State machines inside state machine inside concurrency containers still blocked during execution. * [flexbe_core] Slightly reworked monitoring state diff --git a/flexbe_core/flexbe_core/__init__.py b/flexbe_core/flexbe_core/__init__.py index 32ef254..0f07353 100644 --- a/flexbe_core/flexbe_core/__init__.py +++ b/flexbe_core/flexbe_core/__init__.py @@ -43,22 +43,34 @@ from .logger import Logger # noqa: F401 from .state_logger import StateLogger # noqa: F401 -MIN_UI_VERSION = '4.0.0' # Minimum FlexBE App or UI version required to interact with this version of flexbe_core +MIN_UI_VERSION = '4.1.0' # Minimum FlexBE UI version required to interact with this version of flexbe_core # pylint: disable=R0903 -def set_node(node): +def initialize_flexbe_core(node): """Set node information and initialize classes.""" from .proxy import initialize_proxies # pylint: disable=C0415 from .core import RosState, RosStateMachine # pylint: disable=C0415 Logger.initialize(node) + Logger.localinfo('Initialize ROS enabled classes ...') StateLogger.initialize_ros(node) initialize_proxies(node) RosState.initialize_ros(node) RosStateMachine.initialize_ros(node) +def set_node(node): + """Set node information and initialize classes.""" + import warnings + warnings.warn( + 'set_node function is deprecated and will be removed in a future version. Use initialize_flexbe_core(node) instead!', + DeprecationWarning, + stacklevel=2 + ) + initialize_flexbe_core(node) + + class Autonomy: """Provides constants for the available required Autonomy Levels.""" @@ -93,14 +105,15 @@ class Autonomy: __all__ = [ + 'Autonomy' 'Behavior', 'BehaviorLibrary', 'ConcurrencyContainer', 'EventState', + 'Logger', 'OperatableStateMachine', 'PriorityContainer', - 'Logger', 'StateLogger', - 'set_node', - 'Autonomy' + 'initialize_flexbe_core', + 'set_node', # To be deprecated in future release ] diff --git a/flexbe_core/flexbe_core/behavior.py b/flexbe_core/flexbe_core/behavior.py index b1674c4..bc9cdb0 100644 --- a/flexbe_core/flexbe_core/behavior.py +++ b/flexbe_core/flexbe_core/behavior.py @@ -30,7 +30,7 @@ """This defines the superclass for all implemented behaviors.""" -from flexbe_core.core import LockableStateMachine, OperatableStateMachine, PreemptableState +from flexbe_core.core import LockableStateMachine, OperatableStateMachine, PreemptableState, StateMap from flexbe_core.logger import Logger from flexbe_msgs.msg import BehaviorSync @@ -44,6 +44,7 @@ def __init__(self): self._state_machine = None self.name = 'unnamed behavior' self.beh_id = 0 # Behavior id checksum assigned by processing the file contents + self._state_map = None self.contains = {} self._behaviors = {} @@ -51,7 +52,7 @@ def __init__(self): self._autonomy_level = 3 self._debug = False - self.requested_state_path = None + self.requested_state_id = None # Please implement this abstract method: def create(self): @@ -178,15 +179,25 @@ def set_parameter(self, name, value): def confirm(self): """Confirm that this behavior is ready for execution.""" - LockableStateMachine.path_for_switch = self.requested_state_path - - self._state_machine.confirm(self.name, self.beh_id) - - # def define_structure(self): - # """ - # Calculate all state ids and prepare the ContainerStructure message - # """ - # self._state_machine.define_structure() + self._state_map = StateMap() + self._state_machine.confirm(self.name, self.beh_id, self._state_map) + LockableStateMachine.path_for_switch = None + if self.requested_state_id is not None: + requested_state = self._state_map[self.requested_state_id] + LockableStateMachine.path_for_switch = requested_state.path + + @property + def state_map_items(self): + """Return two lists of keys and values from state map.""" + if self._state_machine is not None: + return list(zip(*self._state_map.items)) + return [], [] + + def get_state_by_id(self, st_id): + """Return state reference from state map by id.""" + if self._state_map is not None: + return self._state_map.get(st_id) + return None def execute(self): """ @@ -225,7 +236,7 @@ def prepare_for_switch(self, state): sm.replace_userdata(state_container.userdata) state_container = state_container._parent states[1].replace_state(state) # add to new state machine - self.requested_state_path = state.path # set start after switch + self.requested_state_id = state.state_id # set start after switch def get_current_states(self): """Get all currently active (sub-)states.""" diff --git a/flexbe_core/flexbe_core/behavior_library.py b/flexbe_core/flexbe_core/behavior_library.py index 8cf2c13..39d5a7e 100644 --- a/flexbe_core/flexbe_core/behavior_library.py +++ b/flexbe_core/flexbe_core/behavior_library.py @@ -131,7 +131,7 @@ def get_behavior(self, be_key): try: return self._behavior_lib[be_key] except KeyError: - Logger.logwarn(f"Did not find ID '{be_key}' in libary, updating...") + Logger.logwarn(f"Did not find ID '{be_key}' in library, updating...") self.parse_packages() return self._behavior_lib.get(be_key, None) @@ -166,12 +166,12 @@ def __find_behavior(): try: return __find_behavior() except StopIteration: - Logger.logwarn("Did not find behavior '%s' in current libary, updating..." % be_name) + Logger.logwarn("Did not find behavior '%s' in current library, updating..." % be_name) self.parse_packages() try: return __find_behavior() except StopIteration: - Logger.logerr("Still cannot find behavior '%s' in libary after update, giving up!" % be_name) + Logger.logerr("Still cannot find behavior '%s' in library after update, giving up!" % be_name) return None, None def count_behaviors(self): diff --git a/flexbe_core/flexbe_core/core/__init__.py b/flexbe_core/flexbe_core/core/__init__.py index 0772e08..c3ae7f1 100644 --- a/flexbe_core/flexbe_core/core/__init__.py +++ b/flexbe_core/flexbe_core/core/__init__.py @@ -31,6 +31,9 @@ from .concurrency_container import ConcurrencyContainer # noqa: F401 from .event_state import EventState # noqa: F401 +from .exceptions import StateError # noqa: F401 +from .exceptions import StateMachineError # noqa: F401 +from .exceptions import UserDataError # noqa: F401 from .lockable_state import LockableState # noqa: F401 from .lockable_state_machine import LockableStateMachine # noqa: F401 from .manually_transitionable_state import ManuallyTransitionableState # noqa: F401 @@ -44,23 +47,28 @@ from .state import State # noqa: F401 from .state_machine import StateMachine # noqa: F401 from .state_map import StateMap # noqa: F401 +from .topics import Topics from .user_data import UserData # noqa: F401 __all__ = [ - 'PreemptableStateMachine', - 'OperatableStateMachine', - 'LockableStateMachine', - 'RosStateMachine', - 'StateMachine', 'ConcurrencyContainer', - 'PriorityContainer', - 'State', - 'RosState', - 'ManuallyTransitionableState', + 'EventState', 'LockableState', - 'PreemptableState', + 'LockableStateMachine', + 'ManuallyTransitionableState', 'OperatableState', - 'EventState', + 'OperatableStateMachine', + 'PreemptableState', + 'PreemptableStateMachine', + 'PriorityContainer', + 'RosState', + 'RosStateMachine', + 'State', + 'StateError', + 'StateMachine', + 'StateMachineError', 'StateMap', - 'UserData' + 'Topics', + 'UserData', + 'UserDataError' ] diff --git a/flexbe_core/flexbe_core/core/concurrency_container.py b/flexbe_core/flexbe_core/core/concurrency_container.py index 76c76f3..c30a299 100644 --- a/flexbe_core/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/flexbe_core/core/concurrency_container.py @@ -35,9 +35,12 @@ It synchronizes its current state with the mirror and supports some control mechanisms. """ from flexbe_core.core.event_state import EventState +from flexbe_core.core.exceptions import StateError, StateMachineError, UserDataError from flexbe_core.core.lockable_state_machine import LockableStateMachine from flexbe_core.core.operatable_state_machine import OperatableStateMachine +from flexbe_core.core.preemptable_state import PreemptableState from flexbe_core.core.priority_container import PriorityContainer +from flexbe_core.core.state import State from flexbe_core.core.topics import Topics from flexbe_core.core.user_data import UserData from flexbe_core.logger import Logger @@ -86,7 +89,7 @@ def get_required_autonomy(self, outcome, state): assert state in self._states, "get required autonomy in ConcurrencyContainer - state doesn't match!" return self._autonomy[state.name][outcome] except Exception as exc: - Logger.error(f"Failure to retrieve autonomy for '{self.name}' in CC - " + Logger.error(f"Failure to retrieve autonomy for '{self.name}' in ConcurrencyContainer - " f" current state label='{self.name}' state='{state.name}' outcome='{outcome}'.") Logger.localerr(f'error={type(exc)} - {exc}') Logger.localerr(f'current_state={self._current_state}') @@ -99,47 +102,51 @@ def _execute_current_state(self): self._current_state = [] # Concurrency container has multiple active states so use list self._manual_transition_requested = None - # Logger.localinfo(f"-concurrency container '{self.name}' is_controlled={self._is_controlled}" - # f" with {len(self._states)} states is entering={self._entering} ") if self._is_controlled and self._sub.has_buffered(Topics._CMD_TRANSITION_TOPIC): - # Special handling in concurrency container - can be either CC or one of several internal states. + # Special handling in concurrency container - can be either ConcurrencyContainer or one of several internal states. command_msg = self._sub.peek_at_buffer(Topics._CMD_TRANSITION_TOPIC) - if command_msg.target == self.name: + if command_msg.target == self.state_id: cmd_msg2 = self._sub.get_from_buffer(Topics._CMD_TRANSITION_TOPIC) # Using here, so clear from buffer assert cmd_msg2 is command_msg, 'Unexpected change in CMD_TRANSITION_TOPIC buffer' - Logger.localinfo(f"-concurrency container '{self.name}' is handling the transition cmd msg={command_msg}") + Logger.localinfo(f"ConcurrencyContainer '{self.path}' is handling the transition cmd msg={command_msg}") self._force_transition = True outcome = self.outcomes[command_msg.outcome] self._manual_transition_requested = outcome self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='transition', - args=[command_msg.target, self.name])) - Logger.localwarn(f"--> Manually triggered outcome {outcome} of concurrency container '{self.name}'") + args=[f'{command_msg.target}', f'{self.state_id}'])) # string[] + Logger.localwarn(f"--> Manually triggered outcome {outcome} of concurrency container '{self.path}'") + self._publish_outcome(outcome) self._returned_outcomes = {} self._current_state = None self._last_outcome = outcome return outcome else: - Logger.localinfo(f"concurrency container '{self.name}' - storing {command_msg} transition request") + Logger.localinfo(f"\x1b[94mConcurrencyContainer '{self.name}' - storing {command_msg} transition request\x1b[0m") self._manual_transition_requested = command_msg + if self._is_controlled and self._last_requested_outcome is not None: + # We have already processed the current state and received an outcome + # We are waiting on outcome confirmation from the OCS + Logger.loginfo_throttle(2.0, f"CC '{self.path}' is waiting on user to confirm outcome") + return None + for state in self._states: if state.name in self._returned_outcomes and self._returned_outcomes[state.name] is not None: # print(f" in current {self._name} : state '{state.name}' is already done.", flush=True) - self._current_state.append(state) continue # already done with executing if self._manual_transition_requested is not None: - if self._manual_transition_requested.target == state.name: + if self._manual_transition_requested.target == state.state_id: # Transition request applies to this state # @TODO - Should we be using path not name here? command_msg = self._manual_transition_requested cmd_msg2 = self._sub.get_from_buffer(Topics._CMD_TRANSITION_TOPIC) # Using here, so clear from buffer assert cmd_msg2 is command_msg, 'Something is up with handling of buffer for CMD_TRANSITION_TOPIC' - Logger.localinfo(f"-concurrency container '{self.name}' state '{state.name}' is handling " + Logger.localinfo(f"ConcurrencyContainer '{self.name}' state '{state.path}' is handling " f"the cmd msg='{command_msg}'") self._manual_transition_requested = None # Reset at this level @@ -150,7 +157,8 @@ def _execute_current_state(self): self._returned_outcomes[state.name] = outcome with UserData(reference=self._userdata, remap=self._remappings[state.name], input_keys=state.input_keys, output_keys=state.output_keys) as userdata: - Logger.localinfo(f" CC '{self}' manual transition and on exit for '{state}'") + Logger.localinfo(f"ConcurrencyContainer '{self}' manual transition" + f" '{outcome}' and on exit for '{state}'") state.on_exit(userdata) # ConcurrencyContainer bypasses normal operatable state handling of manual request, so do that here @@ -158,9 +166,9 @@ def _execute_current_state(self): self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='transition', - args=[command_msg.target, state.name])) + args=[f'{command_msg.target}', f'{state.state_id}'])) Logger.localerr(f'--> Manually triggered outcome {outcome} ({command_msg.outcome}) ' - f"of state '{state.name}' from inside concurrency {self.name}") + f"of state '{state.name}' from inside ConcurrencyContainer '{self.name}'") continue else: Logger.localerr(f"--> Invalid outcome {command_msg.outcome} request for state '{state.name}' " @@ -195,9 +203,9 @@ def _execute_current_state(self): # Determine concurrency outcome outcome = None - if any(self._returned_outcomes[state.name] == state._preempted_name + if any(self._returned_outcomes[state.name] == State._preempted_name for state in self._states if state.name in self._returned_outcomes): - return self._preempted_name # handle preemption if required + return State._preempted_name # handle preemption if required # check conditions for item in self._conditions: (out, cond) = item @@ -211,11 +219,6 @@ def _execute_current_state(self): self._current_state = None if self._is_controlled: - # reset previously requested outcome if applicable - if self._last_requested_outcome != outcome: - self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, OutcomeRequest(outcome=255, target=self.path)) - self._last_requested_outcome = None - # request outcome because autonomy level is too low if (not self._force_transition and self.parent is not None and (not self.parent.is_transition_allowed(self.name, outcome) @@ -223,8 +226,8 @@ def _execute_current_state(self): if outcome != self._last_requested_outcome: self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, OutcomeRequest(outcome=self.outcomes.index(outcome), - target=self.path)) - Logger.localinfo('<-- Want result: %s > %s' % (self.name, outcome)) + target=self.state_id)) + Logger.localinfo('<-- Want result: %s > %s' % (self.path, outcome)) StateLogger.log('flexbe.operator', self, type='request', request=outcome, autonomy=self.parent.autonomy_level, required=self.parent.get_required_autonomy(outcome, self)) @@ -232,7 +235,6 @@ def _execute_current_state(self): outcome = None elif outcome is not None and outcome in self.outcomes: # autonomy level is high enough, report the executed transition - self._publish_outcome(outcome) self._force_transition = False self._last_outcome = outcome @@ -246,17 +248,29 @@ def _execute_single_state(self, state, force_exit=False): input_keys=state.input_keys, output_keys=state.output_keys) as userdata: state._inner_sync_request = False # clear any prior sync on call to individual state if force_exit: - state._entering = True + if state._exited: + Logger.localinfo(f"force exit for '{state.name}' ({state.path}) but already exited?") state.on_exit(userdata) + state._entering = True + state._exited = True + if state._last_outcome is None: + Logger.localinfo(f"preempting '{state.name}' ({state.path})") + state._last_outcome = State._preempted_name + state._publish_outcome(State._preempted_name) # Normally by EventState or StateMachine.execute else: - result = state.execute(userdata) + result = state.execute(userdata) # This is call on_exit if necessary except Exception as exc: # pylint: disable=W0703 + # catch any exception and log here, but re-raise to preempt behavior result = None self._last_exception = exc Logger.logerr('ConcurrencyContainer: Failed to execute state %s:\n%s' % (self.current_state_label, str(exc))) import traceback # pylint: disable=C0415 Logger.localinfo(traceback.format_exc().replace('%', '%%')) - + if isinstance(exc, (StateError, StateMachineError, UserDataError)): + self._last_exception = exc + else: + self._last_exception = StateError(str(exc)) + raise self._last_exception return result def on_enter(self, userdata): # pylint: disable=W0613 @@ -270,38 +284,56 @@ def on_enter(self, userdata): # pylint: disable=W0613 def on_exit(self, userdata, states=None): """Call when concurrency container exits.""" + Logger.localinfo(f"ConcurrencyContainer on_exit for '{self}'.") for state in self._states if states is None else states: if state.name in self._returned_outcomes and self._returned_outcomes[state.name] is not None: + if not state._exited: + Logger.localinfo(f"\x1b[93mCC '{self.name}' - '{state.name}' is in returned outcomes " + f"w/ '{self._returned_outcomes[state.name] }' but has not exited!\x1b[0m") continue # skip states that already exited themselves - Logger.localinfo(f" CC '{self}' exiting contained state '{state}'") self._execute_single_state(state, force_exit=True) + self._current_state = None self._returned_outcomes = {} self._entering = True + if self._last_outcome is None: + # Publish outcome is normally invoked by StateMachine.execute + # If no outcome set, then notify that we preempted this state + self._publish_outcome(State._preempted_name) + + if self._last_requested_outcome is not None: + # Logger.localinfo(f"CC '{self.name}' of '{self.path}' clear prior LRO='{self._last_requested_outcome}'.") + self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, OutcomeRequest(outcome=255, target=self.state_id)) + self._last_requested_outcome = None + + self._exited = True + self._entering = True # for next entry + def get_deep_states(self): """ Recursively look for the currently executing states. Traverse all state machines down to the terminal child state that is not a container. - EXCEPT for ConcurrencyContainers. Those are both active state and container. - @return: The list of active states (not state machine) """ deep_states = [self] # Concurrency acts as both state and container for this purpose - if isinstance(self._current_state, list): - for state in self._current_state: - # Internal states (after skipping concurrency container self) - if isinstance(state, LockableStateMachine): - deep_states.extend(state.get_deep_states()) - else: - deep_states.append(state) - # Logger.localinfo(f"Concurrent get_deep_states: {self.name} {[state.path for state in deep_states]}") - return deep_states - elif self._current_state is not None: - Logger.localerr(f"ConcurrentContainer.get_deep_states '{self.name}' - current state is NOT a list!") - raise TypeError(f"ConcurrentContainer.get_deep_states '{self.name}' - current state is NOT a list!") - # Otherwise, either haven't fully entered, or all have returned outcomes - + for state in self._states: + # Internal states (after skipping concurrency container self) + if isinstance(state, LockableStateMachine): + deep_states.extend(state.get_deep_states()) + else: + deep_states.append(state) return deep_states + + def _notify_skipped(self): + # make sure we dont miss a preempt even if not being executed (e.g., due to priority container) + for state in self._current_state: + # Prioritize handling at low level state first + state._notify_skipped() + + if self._is_controlled and self._sub.has_msg(Topics._CMD_PREEMPT_TOPIC): + self._sub.remove_last_msg(Topics._CMD_PREEMPT_TOPIC) + self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='preempt')) + PreemptableState.preempt = True diff --git a/flexbe_core/flexbe_core/core/event_state.py b/flexbe_core/flexbe_core/core/event_state.py index be838b4..6e3eb47 100644 --- a/flexbe_core/flexbe_core/core/event_state.py +++ b/flexbe_core/flexbe_core/core/event_state.py @@ -87,6 +87,7 @@ def _event_execute(self, *args, **kwargs): if self._entering: self._entering = False + self._exited = False self._last_outcome = None self.on_enter(*args, **kwargs) @@ -104,9 +105,18 @@ def _event_execute(self, *args, **kwargs): self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='repeat')) repeat = True - if repeat or outcome is not None and not PreemptableState.preempt: - self._entering = True + if repeat or outcome is not None: + # As this is currently coded, a repeat command will immediately halt + # call on_exit, then reenter the state + # (vs. an alternative to wait until outcome and then repeat) self.on_exit(*args, **kwargs) + self._exited = True + self._entering = True # for next call + if repeat: + outcome = None # clear outcome so we stay on this state + else: + # Publish outcome for this state + self._publish_outcome(outcome) self._last_outcome = outcome return outcome diff --git a/flexbe_core/flexbe_core/core/lockable_state.py b/flexbe_core/flexbe_core/core/lockable_state.py index 2b752ae..cb430f3 100644 --- a/flexbe_core/flexbe_core/core/lockable_state.py +++ b/flexbe_core/flexbe_core/core/lockable_state.py @@ -37,7 +37,7 @@ from flexbe_msgs.msg import CommandFeedback -from std_msgs.msg import String +from std_msgs.msg import Int32 class LockableState(ManuallyTransitionableState): @@ -99,41 +99,41 @@ def _lockable_execute(self, *args, **kwargs): def _execute_lock(self, target): """Execute lock.""" - if target in (self.path, ''): - target = self.path + if target in (self.state_id, 0): + target = self.state_id found_target = True self._locked = True else: found_target = self._parent.lock(target) # provide feedback about lock self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, - CommandFeedback(command='lock', args=[target, target if found_target else ''])) + CommandFeedback(command='lock', args=[f'{target}', f'{target}' if found_target else '-1'])) if not found_target: - Logger.logwarn(f"Wanted to lock '{target}', but could not find it in current path '{self.path}'.") + Logger.logwarn(f"Wanted to lock state id '{target}', but could not find it in current path '{self.path}'.") else: - Logger.localinfo(f"--> Locking in state '{target}'") + Logger.localinfo(f"--> Locking in state '{target}' at '{self.path}'") def _execute_unlock(self, target): - if target == self.path or (self._locked and target == ''): - target = self.path + if target == self.state_id or (self._locked and target == 0): + target = self.state_id found_target = True self._locked = False else: found_target = self._parent.unlock(target) # provide feedback about unlock self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, - CommandFeedback(command='unlock', args=[target, target if found_target else ''])) + CommandFeedback(command='unlock', args=[f'{target}', f'{target}' if found_target else '-1'])) if not found_target: Logger.logwarn(f"Wanted to unlock '{target}', but could not find it in current path '{self.path}'.") else: - Logger.localinfo(f"--> Unlocking in state '{target}'") + Logger.localinfo(f"--> Unlocking in state '{target}' at '{self.path}'") def _enable_ros_control(self): if not self._is_controlled: super()._enable_ros_control() self._pub.create_publisher(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback) - self._sub.subscribe(Topics._CMD_LOCK_TOPIC, String, inst_id=id(self)) - self._sub.subscribe(Topics._CMD_UNLOCK_TOPIC, String, inst_id=id(self)) + self._sub.subscribe(Topics._CMD_LOCK_TOPIC, Int32, inst_id=id(self)) + self._sub.subscribe(Topics._CMD_UNLOCK_TOPIC, Int32, inst_id=id(self)) def _disable_ros_control(self): if self._is_controlled: diff --git a/flexbe_core/flexbe_core/core/lockable_state_machine.py b/flexbe_core/flexbe_core/core/lockable_state_machine.py index 50ca269..11c87e5 100644 --- a/flexbe_core/flexbe_core/core/lockable_state_machine.py +++ b/flexbe_core/flexbe_core/core/lockable_state_machine.py @@ -95,25 +95,25 @@ def remove_state(self, state): # for locking - def lock(self, path): + def lock(self, st_id): """Lock this state.""" - if path == self.path: + if st_id == self.state_id: self._locked = True return True if self._parent is not None: - return self._parent.lock(path) + return self._parent.lock(st_id) return False - def unlock(self, path): + def unlock(self, st_id): """Unlock this state.""" - if path == self.path: + if st_id == self.state_id: self._locked = False return True if self._parent is not None: - return self._parent.unlock(path) + return self._parent.unlock(st_id) return False diff --git a/flexbe_core/flexbe_core/core/manually_transitionable_state.py b/flexbe_core/flexbe_core/core/manually_transitionable_state.py index 4deb24f..dfb6ba9 100644 --- a/flexbe_core/flexbe_core/core/manually_transitionable_state.py +++ b/flexbe_core/flexbe_core/core/manually_transitionable_state.py @@ -58,11 +58,11 @@ def _manually_transitionable_execute(self, *args, **kwargs): self._manual_transition_requested = None if self._is_controlled and self._sub.has_buffered(Topics._CMD_TRANSITION_TOPIC): command_msg = self._sub.peek_at_buffer(Topics._CMD_TRANSITION_TOPIC) - if command_msg.target == self.name: + if command_msg.target == self.state_id: cmd_msg2 = self._sub.get_from_buffer(Topics._CMD_TRANSITION_TOPIC) assert cmd_msg2 is command_msg, 'Unexpected change in CMD_TRANSITION_TOPIC buffer' self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, - CommandFeedback(command='transition', args=[command_msg.target, self.name])) + CommandFeedback(command='transition', args=[f'{command_msg.target}', f'{self.state_id}'])) self._force_transition = True outcome = self.outcomes[command_msg.outcome] @@ -71,7 +71,7 @@ def _manually_transitionable_execute(self, *args, **kwargs): return outcome else: Logger.loginfo(f"Requested outcome for state '{command_msg.target}' " - f" but this active state is '{self.name}' - keep looking for potential nested state") + f" but this active state is '{self.path}' - keep looking for potential nested state") # otherwise, return the normal outcome self._force_transition = False diff --git a/flexbe_core/flexbe_core/core/operatable_state.py b/flexbe_core/flexbe_core/core/operatable_state.py index 357e70b..9f805ec 100644 --- a/flexbe_core/flexbe_core/core/operatable_state.py +++ b/flexbe_core/flexbe_core/core/operatable_state.py @@ -32,6 +32,7 @@ """OperatableState.""" from flexbe_core.core.preemptable_state import PreemptableState +from flexbe_core.core.state import State from flexbe_core.core.state_map import StateMap from flexbe_core.core.topics import Topics from flexbe_core.logger import Logger @@ -64,9 +65,9 @@ def _operatable_execute(self, *args, **kwargs): outcome = self.__execute(*args, **kwargs) if self._is_controlled: - # reset previously requested outcome if applicable + # reset previously requested outcome if applicable (not reset in on_enter/exit like OSM) if self._last_requested_outcome is not None and outcome is None: - self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, OutcomeRequest(outcome=255, target=self.path)) + self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, OutcomeRequest(outcome=255, target=self.state_id)) self._last_requested_outcome = None # request outcome because autonomy level is too low @@ -74,8 +75,8 @@ def _operatable_execute(self, *args, **kwargs): or outcome is not None and self.is_breakpoint): if outcome != self._last_requested_outcome: self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, - OutcomeRequest(outcome=self.outcomes.index(outcome), target=self.path)) - Logger.localinfo("<-- Want result: '%s' > '%s'" % (self.name, outcome)) + OutcomeRequest(outcome=self.outcomes.index(outcome), target=self.state_id)) + Logger.localinfo("<-- Want result: '%s' > '%s'" % (self.path, outcome)) StateLogger.log('flexbe.operator', self, type='request', request=outcome, autonomy=self.parent.autonomy_level, required=self.parent.get_required_autonomy(outcome, self)) @@ -84,15 +85,26 @@ def _operatable_execute(self, *args, **kwargs): # autonomy level is high enough, report the executed transition elif outcome is not None and outcome in self.outcomes: - self._publish_outcome(outcome) + # Logger.localinfo(f"controlled State '{self.name}' from '{self.path}'" + # f"permitting outcome '{outcome}' {self.__class__.__name__}") self._force_transition = False + return outcome def _publish_outcome(self, outcome): """Update the UI and logs about this outcome.""" - # 0 outcome status denotes no outcome, not index so add +1 for valid outcome (subtract in mirror) + if outcome == State._preempted_name: + # special case of preempted outcome specify max outcome hash + Logger.localinfo('Publish Preempted: State result: %s > %s (%d) (%d) (%s)' + % (self.name, outcome, StateMap._MAX_OUTCOME, self.state_id, self.__class__.__name__)) + self._pub.publish(Topics._OUTCOME_TOPIC, UInt32(data=StateMap.hash(self, StateMap._MAX_OUTCOME))) + self._pub.publish(Topics._DEBUG_TOPIC, String(data='%s > %s' % (self.path, outcome))) + return + outcome_index = self.outcomes.index(outcome) - Logger.localinfo(f"State result: '{self.name}' -> '{outcome}'") + # Logger.localinfo('Publish outcome: State result: %s > %s (%d) (%d) (%s)' + # % (self.name, outcome, outcome_index, self.state_id, self.__class__.__name__)) + # 0 outcome status denotes no outcome, not index so add +1 for valid outcome (subtract in mirror) self._pub.publish(Topics._OUTCOME_TOPIC, UInt32(data=StateMap.hash(self, outcome_index))) self._pub.publish(Topics._DEBUG_TOPIC, String(data='%s > %s' % (self.path, outcome))) if self._force_transition: diff --git a/flexbe_core/flexbe_core/core/operatable_state_machine.py b/flexbe_core/flexbe_core/core/operatable_state_machine.py index d074a8f..9f2465b 100644 --- a/flexbe_core/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/flexbe_core/core/operatable_state_machine.py @@ -32,9 +32,11 @@ """OperatableStateMachine.""" from enum import Enum +from flexbe_core.core.exceptions import StateError, StateMachineError, UserDataError from flexbe_core.core.operatable_state import OperatableState from flexbe_core.core.preemptable_state_machine import PreemptableStateMachine from flexbe_core.core.ros_state import RosState +from flexbe_core.core.state import State from flexbe_core.core.state_map import StateMap from flexbe_core.core.topics import Topics from flexbe_core.core.user_data import UserData @@ -70,7 +72,6 @@ def __init__(self, *args, **kwargs): self._autonomy = {} self._inner_sync_request = False self._last_exception = None - self._state_map = None self._structure = None self._type = OperatableStateMachine.ContainerType.OperatableStateMachine.value @@ -108,16 +109,15 @@ def add(label, state, transitions, autonomy=None, remapping=None): PreemptableStateMachine.add(label, state, transitions, remapping) self._autonomy[label] = autonomy - def define_structure(self): + def define_structure(self, state_map): """Calculate all state ids and prepare the ContainerStructure message.""" - self._state_map = StateMap() - self._structure = self._build_structure_msg() - print(f'\x1b[94mBuilt {self._state_map}\x1b[0m', flush=True) + self._structure = self._build_structure_msg(state_map) + print(f'\x1b[94mBuilt {state_map}\x1b[0m', flush=True) - def _build_structure_msg(self): + def _build_structure_msg(self, state_map): """Create a message to describe the structure of this state machine.""" structure_msg = ContainerStructure() - container_msg = self._add_to_structure_msg(structure_msg, self._state_map) + container_msg = self._add_to_structure_msg(structure_msg, state_map) container_msg.outcomes = self.outcomes structure_msg.behavior_id = self.id return structure_msg @@ -180,25 +180,49 @@ def get_latest_status(self): # execution def _execute_current_state(self): - # catch any exception and keep state active to let operator intervene + self._manual_transition_requested = None + if self._is_controlled and self._sub.has_buffered(Topics._CMD_TRANSITION_TOPIC): + # Special handling in statemachine container + command_msg = self._sub.peek_at_buffer(Topics._CMD_TRANSITION_TOPIC) + + if command_msg.target == self.state_id: + cmd_msg2 = self._sub.get_from_buffer(Topics._CMD_TRANSITION_TOPIC) # Using here, so clear from buffer + assert cmd_msg2 is command_msg, 'Unexpected change in CMD_TRANSITION_TOPIC buffer' + Logger.localinfo(f"Statemachine '{self.name}' from '{self.path}' is " + f"handling the transition cmd msg='{command_msg}'") + + self._force_transition = True + outcome = self.outcomes[command_msg.outcome] + self._manual_transition_requested = outcome + self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, + CommandFeedback(command='transition', + args=[f'{command_msg.target}', f'{self.state_id}'])) + Logger.localwarn(f"--> Manually triggered outcome {outcome} of statemachine '{self.name}'") + self._last_outcome = outcome + return outcome + + if self._is_controlled and self._last_requested_outcome is not None: + # We have already processed the current state and received an outcome + # We are waiting on outcome confirmation from the OCS + Logger.loginfo_throttle(2.0, f"OSM '{self.path}' is waiting on user to confirm outcome") + return None + try: - # --- @TODO remove self._inner_sync_request = False # clear any prior sync request outcome = super()._execute_current_state() self._last_exception = None except Exception as exc: # pylint: disable=W0703 - # Error here - outcome = None - self._last_exception = exc - Logger.logerr('Failed to execute state %s:\n%s' % (self.current_state_label, str(exc))) + # catch any exception and log here, but re-raise to preempt behavior + Logger.logerr("Failed to execute state '%s':\n%s - %s" % (self.current_state_label, str(type(exc)), str(exc))) import traceback # pylint: disable=C0415 - Logger.localinfo(traceback.format_exc().replace('%', '%%')) # Guard against exeception including format! + Logger.localinfo(traceback.format_exc().replace('%', '%%')) # Guard against exception including format! + outcome = None + if isinstance(exc, (StateError, StateMachineError, UserDataError)): + self._last_exception = exc + else: + self._last_exception = StateError(str(exc)) + raise self._last_exception if self._is_controlled: - # reset previously requested outcome if applicable - if self._last_requested_outcome is not None and outcome is None: - self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, OutcomeRequest(outcome=255, target=self.path)) - self._last_requested_outcome = None - # request outcome because autonomy level is too low if not self._force_transition and self.parent is not None: # This check is not relevant to top-level state machines @@ -207,17 +231,15 @@ def _execute_current_state(self): if outcome != self._last_requested_outcome: self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, OutcomeRequest(outcome=self.outcomes.index(outcome), - target=self.path)) - Logger.localinfo("<-- Want result: '%s' -> '%s'" % (self.name, outcome)) + target=self.state_id)) + Logger.localinfo("<-- Want result: '%s' -> '%s'" % (self.path, outcome)) StateLogger.log('flexbe.operator', self, type='request', request=outcome, autonomy=self.parent.autonomy_level, required=self.parent.get_required_autonomy(outcome, self)) self._last_requested_outcome = outcome outcome = None - # autonomy level is high enough, report the executed transition - elif outcome is not None and outcome in self.outcomes: - self._publish_outcome(outcome) + if outcome is not None: self._force_transition = False self._last_outcome = outcome @@ -226,16 +248,17 @@ def _execute_current_state(self): def _publish_outcome(self, outcome): """Update the UI and logs about this outcome.""" # 0 outcome status denotes no outcome, not index so add +1 for valid outcome (subtract in mirror) - try: - outcome_index = self.outcomes.index(outcome) - except Exception as exc: # pylint: disable=W0703 - outcome_index = 0 - Logger.localerr('State Machine outcome error : %s > %s (%d) (%d) (%s)' - % (self.name, outcome, outcome_index, self._state_id, self.__class__.__name__)) - raise exc - - Logger.localinfo('State Machine result: %s > %s (%d) (%d) (%s)' - % (self.name, outcome, outcome_index, self._state_id, self.__class__.__name__)) + if outcome == State._preempted_name: + # special case of preempted outcome specify max outcome hash + Logger.localinfo('Publish Preempted: State Machine result: %s > %s (%d) (%d) (%s)' + % (self.name, outcome, StateMap._MAX_OUTCOME, self.state_id, self.__class__.__name__)) + self._pub.publish(Topics._OUTCOME_TOPIC, UInt32(data=StateMap.hash(self, StateMap._MAX_OUTCOME))) + self._pub.publish(Topics._DEBUG_TOPIC, String(data='%s > %s' % (self.path, outcome))) + return + + outcome_index = self.outcomes.index(outcome) + # Logger.localinfo('Publish outcome: State Machine result: %s > %s (%d) (%d) (%s)' + # % (self.name, outcome, outcome_index, self.state_id, self.__class__.__name__)) self._pub.publish(Topics._OUTCOME_TOPIC, UInt32(data=StateMap.hash(self, outcome_index))) self._pub.publish(Topics._DEBUG_TOPIC, String(data='%s > %s' % (self.path, outcome))) if self._force_transition: @@ -248,13 +271,28 @@ def process_sync_request(self): Provide explicit sync as back-up functionality. Should be used sparingly if there is no other choice - since it requires additional 8 byte + header update bandwith and time to restart mirror + since it requires additional 8 byte + header update bandwidth and time to restart mirror """ if self._inner_sync_request: self._inner_sync_request = False self._pub.publish(Topics._MIRROR_SYNC_TOPIC, self.get_latest_status()) self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='sync', args=[])) Logger.localinfo('<-- Sent synchronization message to mirror.') + + for state in self._last_deep_states_list[::-1]: + # Logger.localinfo(f" '{state.name:30s}' - '{state.path}' " + # f"{f'({state._last_requested_outcome})' if state._last_requested_outcome is not None else ''}") + if state._last_requested_outcome is not None: + if state._last_requested_outcome in state.outcomes: + # Resend outcome request message if resync is requested + self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, + OutcomeRequest(outcome=state.outcomes.index(state._last_requested_outcome), + target=state.state_id)) + Logger.localinfo("<-- Want result: '%s' -> '%s'" % (state.path, state._last_requested_outcome)) + break # only process the deepest requested outcome + else: + Logger.localerr(f" Invalid last requested outcome '{state._last_requested_outcome}'" + f" for '{state}' - '{state.path}'") else: Logger.error('Inner sync processed for %s - but no sync request flag?' % (self.name)) @@ -268,7 +306,7 @@ def get_required_autonomy(self, outcome, state): assert self.current_state_label == state.name, "get required autonomys in OSM state doesn't match!" return self._autonomy[self.current_state_label][outcome] except Exception: # pylint: disable=W0703 - Logger.error(f"Failure to retrieve autonomy for '{self.name}' - " + Logger.error(f"Failure to retrieve autonomy for '{self.name}' - {self.current_state_label}" f" current state label='{self.name}' outcome='{outcome}'.") Logger.localerr(f'{self._autonomy}') @@ -297,7 +335,7 @@ def destroy(self): Logger.localinfo(' state logger shutdown ...') StateLogger.shutdown() - def confirm(self, name, beh_id): + def confirm(self, name, beh_id, state_map): """ Confirm the state machine and triggers the creation of the structural message. @@ -309,11 +347,14 @@ def confirm(self, name, beh_id): @type beh_id: int @param beh_id: The behavior id of this state machine to identify it. + + @type state_map: StateMap + @param state_map: mapping of state ids to state instance """ self.set_name(name) self.id = beh_id - self.define_structure() + self.define_structure(state_map) Logger.localinfo(f"State machine '{self.name}' ({self.id}) (inst_id={id(self)}) confirmed and structure defined.") Logger.localinfo(f'--> Set up pub/sub for behavior {self.name}: {self.id} ...') @@ -416,15 +457,50 @@ def _notify_stop(self): super()._notify_stop() self._structure = None # Flag for destruction + def on_enter(self, userdata=None): # pylint: disable=W0613 + """Call on entering the operatable state machine.""" + self._last_outcome = None + self._last_exception = None + self._last_requested_outcome = None + super().on_enter(userdata) + def on_exit(self, userdata=None): """Call on exiting the statemachine.""" - self._entering = True - if self._current_state is not None: + if self._current_state is None: + if self._exited: + # This should never be true + Logger.localinfo(f"OSM on exit for '{self.name}' from '{self.path}' - " + f'and have already called on_exit? {self._current_state}, ' + f"{self._exited} '{self._last_outcome}' ...") + else: + # Current state is still active, so preempt and call on_exit + if self._current_state._exited or self._current_state._last_outcome is not None: + # This should not be true as should have cleared current state if this was true + Logger.localinfo(f"OSM on exit for '{self.name}' from '{self.path}' (exited={self._exited}) " + f"- with current='{self._current_state}' " + f"(exited={self._current_state._exited}), '{self._current_state._last_outcome}' ...") + with UserData(reference=self._userdata, input_keys=self._current_state.input_keys, output_keys=self._current_state.output_keys, remap=self._remappings[self._current_state.name]) as udata: # Pass userdata to internal states matching as defined in state_machine self._current_state.on_exit(udata) + self._current_state._exited = True + Logger.localinfo(f"preempting '{self._current_state.name}' ({self._current_state.path})") + self._current_state._publish_outcome(State._preempted_name) # Normally published by EventState.execute self._current_state._entering = True self._current_state = None + + if self._last_outcome is None: + # no outcome, so notify that we preempted this state + # otherwise, outcome published by SM execute for regular outcome + self._publish_outcome(State._preempted_name) + + if self._last_requested_outcome is not None: + # Logger.localinfo(f"SM '{self.name}' of '{self.path}' clear prior LRO='{self._last_requested_outcome}'.") + self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, OutcomeRequest(outcome=255, target=self.state_id)) + self._last_requested_outcome = None + + self._exited = True + self._entering = True # for next entry diff --git a/flexbe_core/flexbe_core/core/preemptable_state.py b/flexbe_core/flexbe_core/core/preemptable_state.py index cbc5372..cc6c0d8 100644 --- a/flexbe_core/flexbe_core/core/preemptable_state.py +++ b/flexbe_core/flexbe_core/core/preemptable_state.py @@ -32,6 +32,7 @@ """PreemptableState.""" from flexbe_core.core.lockable_state import LockableState +from flexbe_core.core.state import State from flexbe_core.core.topics import Topics from flexbe_core.logger import Logger @@ -68,7 +69,7 @@ def _preemptable_execute(self, *args, **kwargs): if not self._is_controlled: Logger.localinfo('Behavior will be preempted') self._force_transition = True - return self._preempted_name + return State._preempted_name return self.__execute(*args, **kwargs) diff --git a/flexbe_core/flexbe_core/core/preemptable_state_machine.py b/flexbe_core/flexbe_core/core/preemptable_state_machine.py index dacabe3..03a1396 100644 --- a/flexbe_core/flexbe_core/core/preemptable_state_machine.py +++ b/flexbe_core/flexbe_core/core/preemptable_state_machine.py @@ -34,6 +34,7 @@ from flexbe_core.core.lockable_state_machine import LockableStateMachine from flexbe_core.core.preemptable_state import PreemptableState +from flexbe_core.core.state import State from flexbe_core.core.topics import Topics from flexbe_core.logger import Logger @@ -51,8 +52,6 @@ class PreemptableStateMachine(LockableStateMachine): If preempted, the state machine will return the outcome preempted. """ - _preempted_name = 'preempted' - def __init__(self, *args, **kwargs): """Initialize instance.""" super().__init__(*args, **kwargs) @@ -75,20 +74,27 @@ def _preempt_cb(self, msg): @staticmethod def add(label, state, transitions=None, remapping=None): """Add state to SM.""" - transitions[PreemptableState._preempted_name] = PreemptableStateMachine._preempted_name + transitions[State._preempted_name] = State._preempted_name LockableStateMachine.add(label, state, transitions, remapping) @property def _valid_targets(self): - return super()._valid_targets + [PreemptableStateMachine._preempted_name] + return super()._valid_targets + [State._preempted_name] - def spin(self, userdata=None): + def spin(self, userdata=None, rclpy_context=None): """Spin the execute loop for preemptable portion.""" outcome = None - while rclpy.ok(): + while rclpy.ok(context=rclpy_context): command_msg = self._sub.peek_at_buffer(Topics._CMD_TRANSITION_TOPIC) - outcome = self.execute(userdata) + try: + outcome = self.execute(userdata) + except Exception as exc: + Logger.logerr(f"Exception in '{self.name}' - stopping behavior!") + Logger.localinfo(f'{exc}') + self.on_exit(userdata) # Call to preempt any active states + Logger.logerr(f"Exception in '{self.name}' - {exc}") + return None if command_msg is not None: command_msg2 = self._sub.peek_at_buffer(Topics._CMD_TRANSITION_TOPIC) @@ -98,7 +104,7 @@ def spin(self, userdata=None): f"cmd='{command_msg.target}' ({command_msg.outcome}) - toss it!") self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='transition', - args=['invalid', command_msg.target])) + args=['invalid', f'{command_msg.target}'])) self._sub.get_from_buffer(Topics._CMD_TRANSITION_TOPIC) # Store the information for safely passing to heartbeat thread @@ -122,7 +128,6 @@ def spin(self, userdata=None): break self.wait(seconds=self.sleep_duration) - return outcome def get_latest_status(self): @@ -138,3 +143,14 @@ def get_latest_status(self): def process_sync_request(cls): """Process sync request (ignored here - should be handled by derived state).""" Logger.localinfo('Ignoring PreemptableState process_sync_request') + + def _notify_skipped(self): + # make sure we dont miss a preempt even if not being executed (e.g., due to priority container) + if self._current_state is not None: + # Prioritize handling at low level state first + self._current_state._notify_skipped() + + if self._is_controlled and self._sub.has_msg(Topics._CMD_PREEMPT_TOPIC): + self._sub.remove_last_msg(Topics._CMD_PREEMPT_TOPIC) + self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='preempt')) + PreemptableState.preempt = True diff --git a/flexbe_core/flexbe_core/core/priority_container.py b/flexbe_core/flexbe_core/core/priority_container.py index 2678912..9cbd8e5 100644 --- a/flexbe_core/flexbe_core/core/priority_container.py +++ b/flexbe_core/flexbe_core/core/priority_container.py @@ -31,6 +31,7 @@ """A state machine that is always executed alone when becoming active.""" from flexbe_core.core.operatable_state_machine import OperatableStateMachine +from flexbe_core.logger import Logger class PriorityContainer(OperatableStateMachine): @@ -55,6 +56,19 @@ def execute(self, *args, **kwargs): outcome = OperatableStateMachine.execute(self, *args, **kwargs) if outcome is not None: + # Logger.localinfo(f"Priority container '{self}' outcome='{outcome}' reset active " + # f"from '{PriorityContainer.active_container}'" + # f" to '{self._parent_active_container}'") PriorityContainer.active_container = self._parent_active_container return outcome + + def on_enter(self, userdata=None): # pylint: disable=W0613 + """Call on entering the operatable state machine.""" + Logger.localinfo(f"Enter priority container '{self}' ...") + super().on_enter(userdata) + + def on_exit(self, userdata=None): + """Call on exiting the statemachine.""" + super().on_enter(userdata) + Logger.localinfo(f"Exited priority container '{self}'") diff --git a/flexbe_core/flexbe_core/core/state.py b/flexbe_core/flexbe_core/core/state.py index a655dec..ad66328 100644 --- a/flexbe_core/flexbe_core/core/state.py +++ b/flexbe_core/flexbe_core/core/state.py @@ -47,7 +47,7 @@ class State: _preempted_name = 'preempted' # Define name here, but handle logic in derived class def __init__(self, *args, **kwargs): - """Initilize state instance.""" + """Initialize state instance.""" self._outcomes = _remove_duplicates(kwargs.get('outcomes', [])) io_keys = kwargs.get('io_keys', []) self._input_keys = _remove_duplicates(kwargs.get('input_keys', []) + io_keys) @@ -55,10 +55,11 @@ def __init__(self, *args, **kwargs): # properties of instances of a state machine self._name = None self._parent = None - self._state_id = None # Assigned with structure after all states added to behavior + self._state_id = -1 # Assigned with structure after all states added to behavior self._inner_sync_request = False # Any state can generate request, but should be rare self._type = 0 # Basic states are type 0, containers have non-zero type self._entering = True + self._exited = False # State has exited since last on_enter def __str__(self): """Return name of this state.""" diff --git a/flexbe_core/flexbe_core/core/state_machine.py b/flexbe_core/flexbe_core/core/state_machine.py index 14f806f..439c424 100644 --- a/flexbe_core/flexbe_core/core/state_machine.py +++ b/flexbe_core/flexbe_core/core/state_machine.py @@ -113,7 +113,14 @@ def spin(self, userdata=None): """Spin the SM execute loop.""" outcome = None while True: - outcome = self.execute(userdata) + try: + outcome = self.execute(userdata) + except Exception as exc: + Logger.logerr(f"Exception in '{self}' - stopping behavior!") + Logger.localinfo(f'{exc}') + self.on_exit(userdata) + Logger.logerr(f"Exception in '{self}' - {exc}") + return None if outcome is not None: break @@ -124,22 +131,30 @@ def spin(self, userdata=None): def execute(self, userdata): """Execute the SM.""" - if self._entering or self._current_state is None: - self.assert_consistent_transitions() - self._entering = False - self._current_state = self.initial_state - self._userdata = userdata if userdata is not None else UserData() - self._userdata(add_from=self._own_userdata) - # Logger.localinfo(f"Entering StateMachine '{self.name}' " - # f"({self._state_id}) initial state='{self._current_state.name}'") + if self._entering: + self.on_enter(userdata) + outcome = self._execute_current_state() if outcome: # Exit this statemachine self.on_exit(self._userdata) + self._publish_outcome(outcome) return outcome + def on_enter(self, userdata): + """Call on entering the state machine.""" + self.assert_consistent_transitions() + self._entering = False + self._exited = False + self._current_state = self.initial_state + self._current_state._entering = True # Force entering action + self._userdata = userdata if userdata is not None else UserData() + self._userdata(add_from=self._own_userdata) + # Logger.localinfo(f"Entering StateMachine '{self.name}' of '{self.path}' " + # f"({self.state_id}) initial state='{self._current_state.name}' ({self.__class__.__name__})") + def _execute_current_state(self): """Execute the currently active state in this SM.""" with UserData(reference=self._userdata, remap=self._remappings[self._current_state.name], @@ -219,10 +234,10 @@ def get_deep_states(self): @return: The list of active states (not state machine) """ if isinstance(self._current_state, StateMachine): - return self._current_state.get_deep_states() + return [self] + self._current_state.get_deep_states() # Base case is current_state is not a state machine - return [self._current_state] if self._current_state is not None else [] # Return as a list + return [self, self._current_state] if self._current_state is not None else [self] # Return as a list # consistency checks diff --git a/flexbe_core/flexbe_core/core/state_map.py b/flexbe_core/flexbe_core/core/state_map.py index 130a635..b5bf48a 100644 --- a/flexbe_core/flexbe_core/core/state_map.py +++ b/flexbe_core/flexbe_core/core/state_map.py @@ -38,7 +38,9 @@ class StateMap: __HASH_BITS = 31 # Keep in signed 32-bit range # 2**31 - 1 - 256 - __HASH_MASK = 0x7FFFFF00 # 23-bits keeps in signed range, and allows for 256 output encoding + __HASH_MASK = 0x7FFFFF00 # 23-bits keeps in signed range, and allows for 254 outputs to be encode + + _MAX_OUTCOME = 254 # Hash function adds one, so this is the number of outcomes including preempted def __init__(self): self._state_map = {} @@ -47,7 +49,8 @@ def __init__(self): def __str__(self): """Return string with state map information.""" return (f'State map with {len(self._state_map)} entries' - + ('.' if self._num_collision_processed == 0 else f' and {self._num_collision_processed} collisions!')) + + ('.' if self._num_collision_processed == 0 else + f' (resolved {self._num_collision_processed} state map id collisions)!')) def __getitem__(self, index): """Get existing state if possible, or return None.""" @@ -56,6 +59,11 @@ def __getitem__(self, index): else: return None + @property + def items(self): + """List key-value pairs (index, path).""" + return [(key, state.path) for key, state in self._state_map.items()] + @classmethod def _hash_path(cls, path): """ @@ -86,23 +94,23 @@ def get_path_hash(self, path): def add_state(self, path, state): """Define state id hash and store in map to state instance.""" hash_path = path - if state._state_id is None or state._state_id == -1: + if state.state_id is None or state.state_id == -1: state._state_id = self._hash_path(hash_path) collisions = 0 - while state._state_id in self._state_map: + while state.state_id in self._state_map: # Collision with existing state detected. Define a new state_id by extending path. collisions += 1 if collisions > 20: raise KeyError(f"Unable to avoid collisions in StateMap with '{path}'") hash_path += path state._state_id = self._hash_path(hash_path) - self._state_map[state._state_id] = state + self._state_map[state.state_id] = state self._num_collision_processed += collisions else: - if state._state_id in self._state_map: - Logger.error(f"State '{path}' : id={state._state_id} is already in map!") + if state.state_id in self._state_map: + Logger.error(f"State '{path}' : id={state.state_id} is already in map!") raise KeyError(f"Existing state in StateMap with '{path}'") - self._state_map[state._state_id] = state + self._state_map[state.state_id] = state def get_state(self, state_id): """Return reference to state given id.""" @@ -134,4 +142,4 @@ def unhash(cls, hash_code): @classmethod def hash(cls, state, outcome_index): """Convert state id and outcome to hashed identifier for outcome reports.""" - return state._state_id + 1 + outcome_index + return state.state_id + 1 + outcome_index diff --git a/flexbe_core/flexbe_core/core/topics.py b/flexbe_core/flexbe_core/core/topics.py index 3dfe7ad..ff25646 100644 --- a/flexbe_core/flexbe_core/core/topics.py +++ b/flexbe_core/flexbe_core/core/topics.py @@ -32,8 +32,9 @@ from flexbe_msgs.msg import BEStatus, BehaviorLog, BehaviorRequest, BehaviorSelection, BehaviorSync from flexbe_msgs.msg import CommandFeedback, ContainerStructure from flexbe_msgs.msg import OutcomeRequest +from flexbe_msgs.msg import StateMapMsg -from std_msgs.msg import Bool, Empty, String, UInt32, UInt8 +from std_msgs.msg import Bool, Empty, Int32, String, UInt32, UInt8 class Topics: @@ -47,7 +48,7 @@ class Topics: _CMD_LOCK_TOPIC = 'flexbe/command/lock' # OCS request to lock state _CMD_PAUSE_TOPIC = 'flexbe/command/pause' # OCS request to pause execution _CMD_PREEMPT_TOPIC = 'flexbe/command/preempt' # OCS request behavior preempt - _CMD_REPEAT_TOPIC = 'flexbe/command/repeat' # OCS request to repeat execution of state + _CMD_REPEAT_TOPIC = 'flexbe/command/repeat' # OCS request to repeat execution of a single EventState implementation _CMD_SYNC_TOPIC = 'flexbe/command/sync' # OCS request synchronization _CMD_TRANSITION_TOPIC = 'flexbe/command/transition' # OCS request transition _CMD_UNLOCK_TOPIC = 'flexbe/command/unlock' # OCS request to unlock state @@ -65,23 +66,25 @@ class Topics: _REQUEST_STRUCTURE_TOPIC = 'flexbe/request_mirror_structure' # Request state machine structure from onboard _START_BEHAVIOR_TOPIC = 'flexbe/start_behavior' # OCS or launcher command to start behavior _STATE_LOGGER_TOPIC = 'flexbe/state_logger' + _STATE_MAP_TOPIC = 'flexbe/state_map' # Mapping of state id hash codes to state path from onboard + _STATE_MAP_OCS_TOPIC = 'flexbe/mirror/state_map' # Mapping of state id hash codes to state path from mirror (should be same) _UI_VERSION_TOPIC = 'flexbe/ui_version' # OCS Version topic # Action interfaces _EXECUTE_BEHAVIOR_ACTION = 'flexbe/execute_behavior' _topic_types = {_BEHAVIOR_LOGGING_TOPIC: BehaviorLog, - _BEHAVIOR_UPDATE_TOPIC: String, + _BEHAVIOR_UPDATE_TOPIC: Int32, _CMD_ATTACH_TOPIC: UInt8, _CMD_AUTONOMY_TOPIC: UInt8, _CMD_FEEDBACK_TOPIC: CommandFeedback, - _CMD_LOCK_TOPIC: String, + _CMD_LOCK_TOPIC: Int32, _CMD_PAUSE_TOPIC: Bool, _CMD_PREEMPT_TOPIC: Empty, _CMD_REPEAT_TOPIC: Empty, _CMD_SYNC_TOPIC: Empty, _CMD_TRANSITION_TOPIC: OutcomeRequest, - _CMD_UNLOCK_TOPIC: String, + _CMD_UNLOCK_TOPIC: Int32, _DEBUG_TOPIC: String, _EXECUTE_BEHAVIOR_ACTION: BehaviorExecution, _LAUNCHER_HEARTBEAT_TOPIC: UInt32, @@ -96,6 +99,8 @@ class Topics: _REQUEST_BEHAVIOR_TOPIC: BehaviorRequest, _REQUEST_STRUCTURE_TOPIC: UInt32, _START_BEHAVIOR_TOPIC: BehaviorSelection, + _STATE_MAP_TOPIC: StateMapMsg, + _STATE_MAP_OCS_TOPIC: StateMapMsg, _UI_VERSION_TOPIC: String } diff --git a/flexbe_core/flexbe_core/proxy/proxy_action_client.py b/flexbe_core/flexbe_core/proxy/proxy_action_client.py index 0116399..2248673 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_action_client.py +++ b/flexbe_core/flexbe_core/proxy/proxy_action_client.py @@ -32,6 +32,7 @@ from threading import Lock, Timer from action_msgs.msg import GoalStatus + from flexbe_core.logger import Logger from rclpy.action import ActionClient @@ -51,6 +52,16 @@ class ProxyActionClient: _client_sync_lock = Lock() + _goal_status_dict = { + 0: 'UNKNOWN', + 1: 'ACCEPTED', + 2: 'EXECUTING', + 3: 'CANCELING', + 4: 'SUCCEEDED', + 5: 'CANCELED', + 6: 'ABORTED' + } + @staticmethod def initialize(node): """Initialize ROS setup for proxy action client.""" @@ -77,12 +88,12 @@ def shutdown(): except Exception as exc: # pylint: disable=W0703 print(f'Something went wrong during shutdown of proxy action clients!\n{ str(exc)}') - def __init__(self, topics=None, wait_duration=10): + def __init__(self, topics=None, wait_duration=1.0): """ - Initialize the proxy with optionally a given set of clients. + Initialize the proxy with an optionally given set of clients. @type topics: dictionary string - message class - @param topics: A dictionay containing a collection of topic - message type pairs. + @param topics: A dictionary containing a collection of topic - message type pairs. @type wait_duration: int @param wait_duration: Defines how long to wait for each client in the @@ -93,10 +104,10 @@ def __init__(self, topics=None, wait_duration=10): ProxyActionClient.setup_client(topic, action_type, wait_duration) @classmethod - def setupClient(cls, topic, action_type, wait_duration=10): + def setupClient(cls, topic, action_type, wait_duration=1.0): """Set up proxy action client (Deprecated).""" Logger.localerr('Deprecated: Use ProxyActionClient.setup_client instead!') - cls.setup_client(topic, action_type, wait_duration=10) + cls.setup_client(topic, action_type, wait_duration) @classmethod def setup_client(cls, topic, action_type, wait_duration=None): @@ -266,7 +277,6 @@ def get_result(cls, topic, clear=False): if clear and result is not None: ProxyActionClient._result[topic] = None - ProxyActionClient._result_status[topic] = None return result @@ -279,7 +289,6 @@ def remove_result(cls, topic): @param topic: The topic of interest. """ ProxyActionClient._result[topic] = None - ProxyActionClient._result_status[topic] = None @classmethod def has_feedback(cls, topic): @@ -344,6 +353,22 @@ def get_status(cls, topic): """ return ProxyActionClient._result_status.get(topic) + @classmethod + def get_status_string(cls, topic): + """ + Return the action server status of the given action topic as a string. + + A list of possible states is defined in action_msgs/GoalStatus. + + @type topic: string + @param topic: The topic of interest. + """ + status_code = ProxyActionClient._result_status.get(topic) + if status_code in cls._goal_status_dict: + return cls._goal_status_dict[status_code] + + return 'Unknown Status' + @classmethod def is_active(cls, topic): """ @@ -370,18 +395,24 @@ def cancel(cls, topic): try: current_goal_handle = current_goal_future.result() cancel_future = current_goal_handle.cancel_goal_async() + ProxyActionClient._result_status[topic] = GoalStatus.STATUS_CANCELING + # add callback to acknowledge completion of cancel_goal cancel_future.add_done_callback(partial(ProxyActionClient._cancel_callback, topic=topic)) except Exception as exc: Logger.localinfo(f" Error canceling '{topic}' : {exc}") + else: + Logger.localinfo(f"Failed to send cancel request for '{topic}' ...") ProxyActionClient._current_goal[topic] = None @classmethod def _cancel_callback(cls, future, topic): result = future.result() - Logger.localdebug(f" cancel result for '{topic}' : result={result}") + Logger.localinfo(f" cancel result for '{topic}' : result={result.return_code}") + ProxyActionClient._result_status[topic] = GoalStatus.STATUS_CANCELED + ProxyActionClient._has_active_goal[topic] = False @classmethod def _check_topic_available(cls, topic, wait_duration=0.1): diff --git a/flexbe_core/flexbe_core/proxy/proxy_publisher.py b/flexbe_core/flexbe_core/proxy/proxy_publisher.py index 2236d54..3b3d226 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_publisher.py +++ b/flexbe_core/flexbe_core/proxy/proxy_publisher.py @@ -77,7 +77,7 @@ def __init__(self, topics=None, qos=None, **kwargs): Automatically creates a publisher for sending status messages. @type topics: dictionary string - message class - @param topics: A dictionay containing a collection of topic - message type pairs. + @param topics: A dictionary containing a collection of topic - message type pairs. @type _latch: bool @param: _latch: Defines if messages on the given topics should be latched. diff --git a/flexbe_core/flexbe_core/proxy/proxy_service_caller.py b/flexbe_core/flexbe_core/proxy/proxy_service_caller.py index 3f709b7..5e1fa13 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_service_caller.py +++ b/flexbe_core/flexbe_core/proxy/proxy_service_caller.py @@ -75,7 +75,7 @@ def shutdown(): except Exception as exc: # pylint: disable=W0703 print(f'Something went wrong during shutdown of proxy service caller !\n{str(exc)}') - def __init__(self, topics=None, wait_duration=10): + def __init__(self, topics=None, wait_duration=1.0): """ Initialize the proxy with optionally a given set of clients. @@ -83,14 +83,14 @@ def __init__(self, topics=None, wait_duration=10): @param topics: A dictionary containing a collection of topic - message type pairs. @type wait_duration: float - @param wait_duration: Defines how long to wait (seconds) for the given services if not available right now. + @param wait_duration: Defines how long to wait (seconds) for the given service if not available right now. """ if topics is not None: for topic, srv_type in topics.items(): ProxyServiceCaller.setup_service(topic, srv_type, wait_duration) @classmethod - def setupService(cls, topic, srv_type, wait_duration=10): + def setupService(cls, topic, srv_type, wait_duration=1.0): """Set up the service caller.""" Logger.localerr('Deprecated: Use ProxyServiceCaller.setup_service instead!') cls.setup_service(topic, srv_type, wait_duration) @@ -267,7 +267,7 @@ def result(cls, topic): return ProxyServiceCaller._results[topic].result() @classmethod - def _check_service_available(cls, topic, wait_duration=1): + def _check_service_available(cls, topic, wait_duration=1.0): """ Check whether a service is available. diff --git a/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py b/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py index 77765bc..0a64a1d 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py +++ b/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py @@ -132,7 +132,7 @@ def subscribe(self, topic, msg_type, callback=None, buffered=False, qos=None, in 'callbacks': defaultdict(None), 'subscribers': [inst_id]} - Logger.localinfo(f"Created subscription for '{topic}' with message type '{msg_type.__name__}'!") + # Logger.localinfo(f"Created subscription for '{topic}' with message type '{msg_type.__name__}'!") else: with ProxySubscriberCached._subscription_lock: @@ -143,14 +143,14 @@ def subscribe(self, topic, msg_type, callback=None, buffered=False, qos=None, in # Since we don't throw TypeErrors based on isinstance, and count on Python's duck typing # for callbacks, we will ignore on FlexBE side for subscribers if inst_id not in ProxySubscriberCached._topics[topic]['subscribers']: - Logger.localinfo(f"Add subscriber to existing subscription for '{topic}'" - ' - keep existing subscriber! (' - f"{len(ProxySubscriberCached._topics[topic]['subscribers'])})") + # Logger.localinfo(f"Add subscriber to existing subscription for '{topic}'" + # ' - keep existing subscriber! (' + # f"{len(ProxySubscriberCached._topics[topic]['subscribers'])})") ProxySubscriberCached._topics[topic]['subscribers'].append(inst_id) - else: - Logger.localinfo(f"Existing subscription for '{topic}' with same message type name" - ' - keep existing subscriber! ' - f"({len(ProxySubscriberCached._topics[topic]['subscribers'])})") + # else: + # Logger.localinfo(f"Existing subscription for '{topic}' with same message type name" + # ' - keep existing subscriber! ' + # f"({len(ProxySubscriberCached._topics[topic]['subscribers'])})") else: Logger.info(f'Mis-matched msg_types ({msg_type.__name__} vs. ' f"{ProxySubscriberCached._topics[topic]['subscription'].msg_type.__name__})" @@ -158,13 +158,13 @@ def subscribe(self, topic, msg_type, callback=None, buffered=False, qos=None, in raise TypeError(f"Trying to replace existing subscription with different msg type for '{topic}'") else: if inst_id not in ProxySubscriberCached._topics[topic]['subscribers']: - Logger.localinfo(f"Add subscriber to existing subscription for '{topic}'! " - f"({len(ProxySubscriberCached._topics[topic]['subscribers'])})") + # Logger.localinfo(f"Add subscriber to existing subscription for '{topic}'! " + # f"({len(ProxySubscriberCached._topics[topic]['subscribers'])})") ProxySubscriberCached._topics[topic]['subscribers'].append(inst_id) - else: - Logger.localinfo(f"Existing subscription for '{topic}' with same message type " - '- keep existing subscriber! ' - f"({len(ProxySubscriberCached._topics[topic]['subscribers'])})") + # else: + # Logger.localinfo(f"Existing subscription for '{topic}' with same message type " + # '- keep existing subscriber! ' + # f"({len(ProxySubscriberCached._topics[topic]['subscribers'])})") # Register the local callback for topic message if callback is not None: @@ -368,7 +368,7 @@ def remove_last_msg(cls, topic, clear_buffer=False): @classmethod def make_persistant(cls, topic): """ - Make the given topic persistant which means messages can no longer be removed. + Make the given topic persistent which means messages can no longer be removed. Remove_last_msg will have no effect, only overwritten by a new message. diff --git a/flexbe_core/flexbe_core/proxy/proxy_transform_listener.py b/flexbe_core/flexbe_core/proxy/proxy_transform_listener.py index 03b654e..67132e5 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_transform_listener.py +++ b/flexbe_core/flexbe_core/proxy/proxy_transform_listener.py @@ -51,7 +51,7 @@ def initialize(node): @staticmethod def shutdown(): - """Shut down this proxy by reseting the transform listener.""" + """Shut down this proxy by resetting the transform listener.""" try: ProxyTransformListener._listener = None ProxyTransformListener._buffer = None diff --git a/flexbe_core/package.xml b/flexbe_core/package.xml index dbc72c4..d05612c 100644 --- a/flexbe_core/package.xml +++ b/flexbe_core/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> flexbe_core - 3.0.6 + 4.0.1 flexbe_core provides the core components for the FlexBE behavior engine. diff --git a/flexbe_core/setup.py b/flexbe_core/setup.py index 9010ba3..4874d3a 100644 --- a/flexbe_core/setup.py +++ b/flexbe_core/setup.py @@ -6,7 +6,7 @@ setup( name=PACKAGE_NAME, - version='3.0.6', + version='4.0.1', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]), diff --git a/flexbe_core/test/flexbe_exceptions_spin_test.py b/flexbe_core/test/flexbe_exceptions_spin_test.py new file mode 100644 index 0000000..cc81755 --- /dev/null +++ b/flexbe_core/test/flexbe_exceptions_spin_test.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +"""Test setup.""" +import os +import sys + +import launch + +import launch_testing.actions + +import pytest + + +@pytest.mark.rostest +def generate_test_description(): + """Generate test description for flexbe_exceptions_spin_test.""" + path_to_test = os.path.dirname(__file__) + + test_proc_path = os.path.join(path_to_test, 'test_exceptions_spin.py') + + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env['PYTHONUNBUFFERED'] = '1' + + test_exceptions = launch.actions.ExecuteProcess( + cmd=[sys.executable, test_proc_path], + env=proc_env, + output='screen', + sigterm_timeout=launch.substitutions.LaunchConfiguration('sigterm_timeout', default=15), + sigkill_timeout=launch.substitutions.LaunchConfiguration('sigkill_timeout', default=15) + ) + + return ( + launch.LaunchDescription([ + test_exceptions, + launch_testing.actions.ReadyToTest() + ]), + { + 'test_exceptions': test_exceptions, + } + ) diff --git a/flexbe_core/test/flexbe_logger_test.py b/flexbe_core/test/flexbe_logger_test.py index 69cd4f2..6009743 100644 --- a/flexbe_core/test/flexbe_logger_test.py +++ b/flexbe_core/test/flexbe_logger_test.py @@ -42,7 +42,7 @@ @pytest.mark.rostest def generate_test_description(): - """Generate test descriptoin for flexbe_logger test.""" + """Generate test description for flexbe_logger test.""" path_to_test = os.path.dirname(__file__) TEST_PROC_PATH = os.path.join(path_to_test, 'test_logger.py') diff --git a/flexbe_core/test/test_core.py b/flexbe_core/test/test_core.py index 660cbf7..af30d59 100755 --- a/flexbe_core/test/test_core.py +++ b/flexbe_core/test/test_core.py @@ -33,18 +33,18 @@ import time import unittest -from flexbe_core import ConcurrencyContainer, EventState, OperatableStateMachine -from flexbe_core.core import PreemptableState -from flexbe_core.core.exceptions import StateMachineError -from flexbe_core.core.topics import Topics -from flexbe_core.proxy import ProxySubscriberCached, initialize_proxies, shutdown_proxies +from flexbe_core import ConcurrencyContainer, EventState, OperatableStateMachine, initialize_flexbe_core +from flexbe_core.core import PreemptableState, State +from flexbe_core.core import StateMachineError +from flexbe_core.core import Topics +from flexbe_core.proxy import ProxySubscriberCached, shutdown_proxies from flexbe_msgs.msg import CommandFeedback, OutcomeRequest import rclpy from rclpy.executors import MultiThreadedExecutor -from std_msgs.msg import Bool, Empty, String, UInt32 +from std_msgs.msg import Bool, Empty, Int32, UInt32 class CoreTestState(EventState): @@ -96,6 +96,7 @@ class ConcurrencyTestState(CoreTestState): def __init__(self): super().__init__() + # def execute(self, userdata): # outcome = super().execute(userdata) # self._node.get_logger().info(' %s - ConcurrencyTestState execute' @@ -116,6 +117,10 @@ class TestCore(unittest.TestCase): test = 0 + __EXECUTE_TIMEOUT_SEC = 0.2 # 0.025 # Timeout in executor loops for spin once + __TIME_SLEEP = 0.2 # 0.025 # Sleep time for loops + __LOOP_COUNT = 50 # Number of times to execute loops for checking (total time ~ LOOP_COUNT*(TIME_SLEEP + TIMEOUT)) + def __init__(self, *args, **kwargs): """Initialize TestCore instance.""" super().__init__(*args, **kwargs) @@ -132,40 +137,40 @@ def setUp(self): # self.node.get_logger().info(' set up core test %d (%d)... ' % (self.test, self.context.ok())) self.executor.add_node(self.node) - initialize_proxies(self.node) + initialize_flexbe_core(self.node) - time.sleep(0.1) + time.sleep(TestCore.__TIME_SLEEP) def tearDown(self): """Tear down the TestCore test.""" self.node.get_logger().info(' shutting down core test %d ... ' % (self.test)) - for _ in range(50): + for _ in range(int(0.25 * TestCore.__LOOP_COUNT)): # Allow any lingering pub/sub to clear up - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.01) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) self.node.get_logger().info(' shutting down proxies in core test %d ... ' % (self.test)) shutdown_proxies() - time.sleep(0.1) + time.sleep(TestCore.__TIME_SLEEP) self.node.get_logger().info(' destroy node in core test %d ... ' % (self.test)) self.node.destroy_node() - time.sleep(0.1) + time.sleep(TestCore.__TIME_SLEEP) self.executor.shutdown() - time.sleep(0.1) + time.sleep(TestCore.__TIME_SLEEP) # Kill it with fire to make sure not stray published topics are available rclpy.shutdown(context=self.context) - time.sleep(0.2) + time.sleep(TestCore.__TIME_SLEEP * 2) def _create(self): """Create the test.""" - CoreTestState.initialize_ros(self.node) - OperatableStateMachine.initialize_ros(self.node) + initialize_flexbe_core(self.node) state = CoreTestState() state._enable_ros_control() sm = OperatableStateMachine(outcomes=['done', 'error']) sm._state_id = 1024 + sm.set_name('top-level') with sm: OperatableStateMachine.add('subject', state, transitions={'done': 'done', 'error': 'error'}, @@ -173,26 +178,56 @@ def _create(self): return state, sm def _execute(self, state): - """Execute the test.""" + """Execute the state via container.""" # self.node.get_logger().info(' execute %s ... ' % (str(state.name))) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) state.last_events = [] outcome = state.parent.execute(None) # self.node.get_logger().info(' outcome = %s ... ' % (str(outcome))) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) return outcome + def _reset_sm(self, sm, state): + """Reset the state machine for another pass.""" + if sm is not None: + sm._entering = True + sm.on_enter() + if state is not None: + state._entering = True + state.on_enter(None) + + def _execute_state(self, state): + """ + Execute the state. + + For this, you may need to call on_enter and on_exit. + """ + try: + # self.node.get_logger().info(' execute %s ... ' % (str(state.name))) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.05) + + state.last_events = [] + outcome = state.execute(None) + # self.node.get_logger().info(' outcome = %s ... ' % (str(outcome))) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) + return outcome + except Exception as exc: + self.node.get_logger().error(f" exception in state execute for '{state.name}' ... ") + self.node.get_logger().info(f'{exc}') + import traceback + self.node.get_logger().info(f'{traceback.format_exc()}') + def assertMessage(self, sub, topic, msg, timeout=1): """Check message.""" - for _ in range(int(timeout * 100)): - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + for _ in range(int(timeout * TestCore.__LOOP_COUNT)): + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) if sub.has_msg(topic): received = sub.get_last_msg(topic) sub.remove_last_msg(topic) break - time.sleep(0.1) + time.sleep(TestCore.__TIME_SLEEP) else: raise AssertionError('Did not receive message on topic %s, expected:\n%s' % (topic, str(msg))) @@ -215,27 +250,29 @@ def assertMessage(self, sub, topic, msg, timeout=1): def assertNoMessage(self, sub, topic, timeout=1): """Assert no message received.""" - for _ in range(int(timeout * 100)): - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + if not sub.has_msg(topic): + self.node.get_logger().info(f"Wait to verify no message arrives for '{topic}' ...") + for _ in range(int(timeout * TestCore.__LOOP_COUNT)): + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) if sub.has_msg(topic): received = sub.get_last_msg(topic) sub.remove_last_msg(topic) raise AssertionError('Should not receive message on topic %s, but got:\n%s' % (topic, str(received))) - time.sleep(0.1) + time.sleep(TestCore.__TIME_SLEEP) + self.node.get_logger().info(f" No message arrived for '{topic}' - good!") # Test Cases def test_event_state(self): """Test event state.""" self.node.get_logger().info('test_event_state ... ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - ProxySubscriberCached.initialize(self.node) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) state, sm = self._create() fb_topic = Topics._CMD_FEEDBACK_TOPIC - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) sub = ProxySubscriberCached({fb_topic: CommandFeedback}, inst_id=id(self)) time.sleep(0.2) @@ -286,8 +323,7 @@ def test_operatable_state(self): """Test operatable state.""" self.node.get_logger().info('test_operatable_state ... ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - ProxySubscriberCached.initialize(self.node) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) state, sm = self._create() self.node.get_logger().info('test_operatable_state - ProxySubscribe request ...') out_topic = Topics._OUTCOME_TOPIC @@ -296,54 +332,57 @@ def test_operatable_state(self): # wait for pub/sub end_time = time.time() + 1 while time.time() < end_time: - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) - time.sleep(0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) + time.sleep(TestCore.__TIME_SLEEP) + + state.on_enter(None) # return outcome in full autonomy, no request - self.node.get_logger().info('test_operatable_state - request outcome on full autonomy, no request ...') + self.node.get_logger().info(f'test_operatable_state - autonomy level={OperatableStateMachine.autonomy_level} ' + f'request outcome on full autonomy, no request ...') state.result = 'error' - self._execute(state) + self._execute_state(state) # executing single state to we only get the state outputs self.assertNoMessage(sub, req_topic) - self.assertMessage(sub, out_topic, UInt32(data=(2 + state._state_id))) # Test presumes outcome = 1 + offset + self.assertMessage(sub, out_topic, UInt32(data=(2 + state.state_id))) # Test presumes outcome = 1 + offset # request outcome on same autonomy and clear request on loopback self.node.get_logger().info('test_operatable_state - request outcome on autonomy level=2 ...') + self._reset_sm(sm, state) OperatableStateMachine.autonomy_level = 2 - self._execute(state) + self._execute_state(state) # executing single state to we only get the state outputs self.assertNoMessage(sub, out_topic) - self.assertMessage(sub, req_topic, OutcomeRequest(outcome=1, target='/subject')) + self.assertMessage(sub, req_topic, OutcomeRequest(outcome=1, target=CoreTestState._set_state_id)) state.result = None - self._execute(state) - self.assertMessage(sub, req_topic, OutcomeRequest(outcome=255, target='/subject')) + self._execute_state(state) # executing single state to we only get the state outputs + self.assertMessage(sub, req_topic, OutcomeRequest(outcome=255, target=CoreTestState._set_state_id)) # still return other outcomes self.node.get_logger().info('test_operatable_state - still return other outcomes') state.result = 'done' - self._execute(state) + self._execute_state(state) # executing single state to we only get the state outputs self.assertNoMessage(sub, req_topic) - self.assertMessage(sub, out_topic, UInt32(data=(1 + state._state_id))) # Test presumes outcome = 0 + offset + self.assertMessage(sub, out_topic, UInt32(data=(1 + state.state_id))) # Test presumes outcome = 0 + offset # request outcome on lower autonomy, return outcome after level increase self.node.get_logger().info('test_operatable_state - lower autonomy level=0') OperatableStateMachine.autonomy_level = 0 - self._execute(state) + self._execute_state(state) # executing single state to we only get the state outputs self.assertNoMessage(sub, out_topic) - self.assertMessage(sub, req_topic, OutcomeRequest(outcome=0, target='/subject')) + self.assertMessage(sub, req_topic, OutcomeRequest(outcome=0, target=CoreTestState._set_state_id)) OperatableStateMachine.autonomy_level = 3 self.node.get_logger().info('test_operatable_state -autonomy level=3') - self._execute(state) - self.assertMessage(sub, out_topic, UInt32(data=(1 + state._state_id))) # Test presumes outcome = 0 + offset + self._execute_state(state) + self.assertMessage(sub, out_topic, UInt32(data=(1 + state.state_id))) # Test presumes outcome = 0 + offset self.node.get_logger().info('test_operatable_state - OK! ') def test_preemptable_state(self): """Test preemptable state.""" self.node.get_logger().info('test_preemptable_state ... ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - ProxySubscriberCached.initialize(self.node) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) state, sm = self._create() fb_topic = Topics._CMD_FEEDBACK_TOPIC @@ -353,15 +392,15 @@ def test_preemptable_state(self): # wait for pub/sub end_time = time.time() + 1 while time.time() < end_time: - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) - time.sleep(0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) + time.sleep(TestCore.__TIME_SLEEP) # time.sleep(0.2) # preempt when trigger variable is set PreemptableState.preempt = True self.node.get_logger().info('test_preemptable_state - preempt = True ...') outcome = self._execute(state) - self.assertEqual(outcome, PreemptableState._preempted_name) + self.assertEqual(outcome, State._preempted_name) self.assertRaises(StateMachineError, lambda: state.parent.current_state) self.node.get_logger().info('test_preemptable_state - preempt = False ...') @@ -374,7 +413,7 @@ def test_preemptable_state(self): state._sub._callback(Empty(), Topics._CMD_PREEMPT_TOPIC) outcome = self._execute(state) - self.assertEqual(outcome, PreemptableState._preempted_name) + self.assertEqual(outcome, State._preempted_name) self.assertRaises(StateMachineError, lambda: state.parent.current_state) self.assertMessage(sub, fb_topic, CommandFeedback(command='preempt')) @@ -385,8 +424,7 @@ def test_lockable_state(self): """Test lockable state.""" self.node.get_logger().info('test_lockable_state ... ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - ProxySubscriberCached.initialize(self.node) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) state, sm = self._create() fb_topic = Topics._CMD_FEEDBACK_TOPIC sub = ProxySubscriberCached({fb_topic: CommandFeedback}, inst_id=id(self)) @@ -394,55 +432,55 @@ def test_lockable_state(self): # lock and unlock as commanded, return outcome after unlock self.node.get_logger().info(' test lock on command ... ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - state._sub._callback(String(data='/subject'), Topics._CMD_LOCK_TOPIC) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) + state._sub._callback(Int32(data=state.state_id), Topics._CMD_LOCK_TOPIC) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) state.result = 'done' self.node.get_logger().info(' execute state after lock ... ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) outcome = self._execute(state) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) self.node.get_logger().info(' check results of test lock after execute ... ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) self.assertIsNone(outcome) self.assertTrue(state._locked) - self.assertMessage(sub, fb_topic, CommandFeedback(command='lock', args=['/subject', '/subject'])) + self.assertMessage(sub, fb_topic, CommandFeedback(command='lock', args=[f'{state.state_id}', f'{state.state_id}'])) state.result = None self.node.get_logger().info(' test unlock on command ... ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - state._sub._callback(String(data='/subject'), Topics._CMD_UNLOCK_TOPIC) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) + state._sub._callback(Int32(data=state.state_id), Topics._CMD_UNLOCK_TOPIC) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) outcome = self._execute(state) self.assertEqual(outcome, 'done') - self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=['/subject', '/subject'])) + self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=[f'{state.state_id}', f'{state.state_id}'])) # lock and unlock without target self.node.get_logger().info(' test lock and unlock without target ... ') - state._sub._callback(String(data=''), Topics._CMD_LOCK_TOPIC) + state._sub._callback(Int32(data=0), Topics._CMD_LOCK_TOPIC) state.result = 'done' outcome = self._execute(state) self.assertIsNone(outcome) - self.assertMessage(sub, fb_topic, CommandFeedback(command='lock', args=['/subject', '/subject'])) - state._sub._callback(String(data=''), Topics._CMD_UNLOCK_TOPIC) + self.assertMessage(sub, fb_topic, CommandFeedback(command='lock', args=[f'{state.state_id}', f'{state.state_id}'])) + state._sub._callback(Int32(data=0), Topics._CMD_UNLOCK_TOPIC) outcome = self._execute(state) self.assertEqual(outcome, 'done') - self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=['/subject', '/subject'])) + self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=[f'{state.state_id}', f'{state.state_id}'])) # reject invalid lock command self.node.get_logger().info(' test reject invalid lock command ... ') - state._sub._callback(String(data='/invalid'), Topics._CMD_LOCK_TOPIC) + state._sub._callback(Int32(data=12345678), Topics._CMD_LOCK_TOPIC) # give invalid state id outcome = self._execute(state) self.assertEqual(outcome, 'done') - self.assertMessage(sub, fb_topic, CommandFeedback(command='lock', args=['/invalid', ''])) + self.assertMessage(sub, fb_topic, CommandFeedback(command='lock', args=['12345678', '-1'])) # reject generic unlock command when not locked self.node.get_logger().info(' test reject invalid unlock when not locked command ... ') - state._sub._callback(String(data=''), Topics._CMD_UNLOCK_TOPIC) + state._sub._callback(Int32(data=0), Topics._CMD_UNLOCK_TOPIC) self._execute(state) - self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=['', ''])) + self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=['0', '-1'])) # do not transition out of locked container self.node.get_logger().info(' test do not transition out of locked container ... ') @@ -459,24 +497,25 @@ def test_manually_transitionable_state(self): """Test manually transitionable state.""" self.node.get_logger().info('test_manually_transitionable_state ...') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - ProxySubscriberCached.initialize(self.node) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) state, sm = self._create() fb_topic = Topics._CMD_FEEDBACK_TOPIC sub = ProxySubscriberCached({fb_topic: CommandFeedback}, inst_id=id(self)) time.sleep(0.2) # return requested outcome - state._sub._callback(OutcomeRequest(target='subject', outcome=1), Topics._CMD_TRANSITION_TOPIC) + state._sub._callback(OutcomeRequest(target=CoreTestState._set_state_id, outcome=1), Topics._CMD_TRANSITION_TOPIC) outcome = self._execute(state) self.assertEqual(outcome, 'error') - self.assertMessage(sub, fb_topic, CommandFeedback(command='transition', args=['subject', 'subject'])) + self.assertMessage(sub, fb_topic, CommandFeedback(command='transition', + args=[f'{CoreTestState._set_state_id}', + f'{CoreTestState._set_state_id}'])) # reject outcome request for different target - state._sub._callback(OutcomeRequest(target='invalid', outcome=1), Topics._CMD_TRANSITION_TOPIC) + state._sub._callback(OutcomeRequest(target=42, outcome=1), Topics._CMD_TRANSITION_TOPIC) outcome = self._execute(state) self.assertIsNone(outcome) - # This state won't handle the transition request, so not message is expected + # This state won't handle the transition request, so no message is expected self.assertNoMessage(sub, fb_topic) self.node.get_logger().info('test_manually_transitionable_state - OK! ') @@ -484,7 +523,7 @@ def test_cross_combinations(self): """Test cross combinations.""" self.node.get_logger().info('test_cross_combinations ...') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) state, sm = self._create() # manual transition works on low autonomy @@ -492,7 +531,7 @@ def test_cross_combinations(self): state.result = 'error' outcome = self._execute(state) self.assertIsNone(outcome) - state._sub._callback(OutcomeRequest(target='subject', outcome=0), Topics._CMD_TRANSITION_TOPIC) + state._sub._callback(OutcomeRequest(target=CoreTestState._set_state_id, outcome=0), Topics._CMD_TRANSITION_TOPIC) outcome = self._execute(state) self.assertEqual(outcome, 'done') OperatableStateMachine.autonomy_level = 3 @@ -500,13 +539,13 @@ def test_cross_combinations(self): # manual transition blocked by lock self.node.get_logger().info('test_cross_combinations - check manual transition blocked by lock ... ') - state._sub._callback(String(data='/subject'), Topics._CMD_LOCK_TOPIC) + state._sub._callback(Int32(data=state.state_id), Topics._CMD_LOCK_TOPIC) outcome = self._execute(state) self.assertIsNone(outcome) - state._sub._callback(OutcomeRequest(target='subject', outcome=1), Topics._CMD_TRANSITION_TOPIC) + state._sub._callback(OutcomeRequest(target=CoreTestState._set_state_id, outcome=1), Topics._CMD_TRANSITION_TOPIC) outcome = self._execute(state) self.assertIsNone(outcome) - state._sub._callback(String(data='/subject'), Topics._CMD_UNLOCK_TOPIC) + state._sub._callback(Int32(data=state.state_id), Topics._CMD_UNLOCK_TOPIC) outcome = self._execute(state) self.assertEqual(outcome, 'error') @@ -518,21 +557,21 @@ def test_cross_combinations(self): self.assertIsNone(outcome) state._sub._callback(Empty(), Topics._CMD_PREEMPT_TOPIC) outcome = self._execute(state) - self.assertEqual(outcome, PreemptableState._preempted_name) + self.assertEqual(outcome, State._preempted_name) PreemptableState.preempt = False OperatableStateMachine.autonomy_level = 3 state.result = None # preempt also works when locked self.node.get_logger().info('test_cross_combinations - verify preempt works when locked ... ') - state._sub._callback(String(data='/subject'), Topics._CMD_LOCK_TOPIC) + state._sub._callback(Int32(data=state.state_id), Topics._CMD_LOCK_TOPIC) outcome = self._execute(state) self.assertIsNone(outcome) state._sub._callback(Empty(), Topics._CMD_PREEMPT_TOPIC) outcome = self._execute(state) - self.assertEqual(outcome, PreemptableState._preempted_name) + self.assertEqual(outcome, State._preempted_name) PreemptableState.preempt = False - state._sub._callback(String(data='/subject'), Topics._CMD_UNLOCK_TOPIC) + state._sub._callback(Int32(data=state.state_id), Topics._CMD_UNLOCK_TOPIC) outcome = self._execute(state) self.assertIsNone(outcome) self.node.get_logger().info('test_cross_combinations - OK! ') @@ -540,9 +579,7 @@ def test_cross_combinations(self): def test_concurrency_container(self): """Test CC.""" self.node.get_logger().info('test_concurrency_container ... ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - ConcurrencyTestState.initialize_ros(self.node) - ConcurrencyContainer.initialize_ros(self.node) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) cc = ConcurrencyContainer(outcomes=['done', 'error'], conditions=[ ('error', [('main', 'error')]), @@ -563,7 +600,7 @@ def test_concurrency_container(self): autonomy={'done': 1, 'error': 2}) # self.node.get_logger().info(' after setting up OSM - call execute CC ... ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) # all states are called with their correct rate cc.execute(None) @@ -676,7 +713,7 @@ def test_user_data(self): """Test user data.""" self.node.get_logger().info('test_user_data ...') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) class TestUserdataState(EventState): """Local Test state.""" @@ -685,6 +722,8 @@ def __init__(self, out_content='test_data'): super(TestUserdataState, self).__init__(outcomes=['done'], input_keys=['data_in'], output_keys=['data_out']) self.data = None self._out_content = out_content + CoreTestState._set_state_id = CoreTestState._set_state_id + 1 + self._state_id = CoreTestState._set_state_id def execute(self, userdata): self._node.get_logger().warn('\033[0m%s\n%s' % (self.path, str(userdata))) # log for manual inspection @@ -692,33 +731,40 @@ def execute(self, userdata): userdata.data_out = self._out_content return 'done' - TestUserdataState.initialize_ros(self.node) - OperatableStateMachine.initialize_ros(self.node) inner_sm = OperatableStateMachine(outcomes=['done'], input_keys=['sm_in'], output_keys=['sm_out']) inner_sm._state_id = 4096 inner_sm.userdata.own = 'own_data' with inner_sm: OperatableStateMachine.add('own_state', TestUserdataState('inner_data'), transitions={'done': 'outside_state'}, - remapping={'data_in': 'own', 'data_out': 'sm_out'}) + remapping={'data_in': 'own', 'data_out': 'sm_out'}, + autonomy={'done': 0}) OperatableStateMachine.add('outside_state', TestUserdataState(), transitions={'done': 'internal_state'}, - remapping={'data_in': 'sm_in', 'data_out': 'data_in'}) + remapping={'data_in': 'sm_in', 'data_out': 'data_in'}, + autonomy={'done': 0}) OperatableStateMachine.add('internal_state', TestUserdataState(), transitions={'done': 'done'}, - remapping={}) + remapping={}, + autonomy={'done': 0}) sm = OperatableStateMachine(outcomes=['done']) sm._state_id = 8192 sm.userdata.outside = 'outside_data' + sm.set_name('outer_sm') # No one sets this name, while .add sets names of inner states with sm: OperatableStateMachine.add('before_state', TestUserdataState(), transitions={'done': 'inner_sm'}, - remapping={'data_in': 'outside'}) + remapping={'data_in': 'outside'}, + autonomy={'done': 0}) OperatableStateMachine.add('inner_sm', inner_sm, transitions={'done': 'after_state'}, - remapping={'sm_in': 'outside'}) + remapping={'sm_in': 'outside'}, + autonomy={'done': 0}) OperatableStateMachine.add('after_state', TestUserdataState('last_data'), transitions={'done': 'modify_state'}, - remapping={'data_in': 'sm_out'}) + remapping={'data_in': 'sm_out'}, + autonomy={'done': 0}) OperatableStateMachine.add('modify_state', TestUserdataState(), transitions={'done': 'final_state'}, - remapping={'data_out': 'outside', 'data_in': 'outside'}) + remapping={'data_out': 'outside', 'data_in': 'outside'}, + autonomy={'done': 0}) OperatableStateMachine.add('final_state', TestUserdataState(), transitions={'done': 'done'}, - remapping={'data_in': 'data_out'}) + remapping={'data_in': 'data_out'}, + autonomy={'done': 0}) # can pass userdata to state and get it from state sm.execute(None) diff --git a/flexbe_core/test/test_exceptions.py b/flexbe_core/test/test_exceptions.py index d5d27d4..4aefd02 100755 --- a/flexbe_core/test/test_exceptions.py +++ b/flexbe_core/test/test_exceptions.py @@ -33,9 +33,9 @@ import time import unittest -from flexbe_core import EventState, OperatableStateMachine +from flexbe_core import EventState, OperatableStateMachine, initialize_flexbe_core from flexbe_core.core.exceptions import StateError, StateMachineError, UserDataError -from flexbe_core.proxy import initialize_proxies, shutdown_proxies +from flexbe_core.proxy import shutdown_proxies import rclpy from rclpy.executors import MultiThreadedExecutor @@ -45,6 +45,8 @@ class TestExceptions(unittest.TestCase): """Test FlexBE Exception handling.""" test = 0 + __EXECUTE_TIMEOUT_SEC = 0.2 # 0.025 # Timeout in executor loops for spin once + __TIME_SLEEP = 0.2 # 0.025 # Sleep time for loops def __init__(self, *args, **kwargs): """Initialize TestExceptions instance.""" @@ -60,7 +62,7 @@ def setUp(self): self.node = rclpy.create_node('exception_test_' + str(self.test), context=self.context) self.node.get_logger().info(' set up exceptions test %d (%d) ... ' % (self.test, self.context.ok())) self.executor.add_node(self.node) - initialize_proxies(self.node) + initialize_flexbe_core(self.node) def tearDown(self): """Tear down the TestExceptions test.""" @@ -69,42 +71,45 @@ def tearDown(self): self.node.get_logger().info(' shutting down proxies in core test %d ... ' % (self.test)) shutdown_proxies() - time.sleep(0.1) + time.sleep(TestExceptions.__TIME_SLEEP) self.node.get_logger().info(' destroy node in core test %d ... ' % (self.test)) self.node.destroy_node() - time.sleep(0.1) + time.sleep(TestExceptions.__TIME_SLEEP) self.executor.shutdown() - time.sleep(0.1) + time.sleep(TestExceptions.__TIME_SLEEP) # Kill it with fire to make sure not stray published topics are available rclpy.shutdown(context=self.context) - time.sleep(0.2) + time.sleep(TestExceptions.__TIME_SLEEP * 2) def test_invalid_outcome(self): """Test invalid outcome.""" self.node.get_logger().info('test_invalid_outcome ...') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - OperatableStateMachine.initialize_ros(self.node) - node = self.node + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class ReturnInvalidOutcomeState(EventState): """Local Test state definition.""" def __init__(self): - self.initialize_ros(node) super().__init__(outcomes=['done']) def execute(self, userdata): return 'invalid' - sm = OperatableStateMachine(outcomes=['done']) - with sm: - OperatableStateMachine.add('state', ReturnInvalidOutcomeState(), transitions={'done': 'done'}) + outcome = None + try: + sm = OperatableStateMachine(outcomes=['done']) + with sm: + OperatableStateMachine.add('state', ReturnInvalidOutcomeState(), + transitions={'done': 'done'}, + autonomy={'done': 0}) + outcome = sm.execute(None) + except StateError as exc: + self.node.get_logger().info(f" sm had expected exception '{exc}'") - outcome = sm.execute(None) self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, StateError) self.node.get_logger().info('test_invalid_outcome - OK! ') @@ -113,15 +118,12 @@ def test_invalid_transition(self): """Test invalid transition.""" self.node.get_logger().info('test_invalid_transition ...') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - OperatableStateMachine.initialize_ros(self.node) - node = self.node + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class ReturnDoneState(EventState): """Local Test state definition.""" def __init__(self): - ReturnDoneState.initialize_ros(node) super().__init__(outcomes=['done']) def execute(self, userdata): @@ -129,12 +131,19 @@ def execute(self, userdata): inner_sm = OperatableStateMachine(outcomes=['done']) with inner_sm: - OperatableStateMachine.add('state', ReturnDoneState(), transitions={'done': 'invalid'}) + OperatableStateMachine.add('state', ReturnDoneState(), + transitions={'done': 'invalid'}, + autonomy={'done': 0}) sm = OperatableStateMachine(outcomes=['done']) with sm: OperatableStateMachine.add('inner', inner_sm, transitions={'done': 'done'}) - outcome = sm.execute(None) + outcome = None + try: + outcome = sm.execute(None) + except StateMachineError as exc: + self.node.get_logger().info(f" sm had expected exception '{exc}'") + self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, StateMachineError) self.node.get_logger().info('test_invalid_transition - OK! ') @@ -143,15 +152,12 @@ def test_invalid_userdata_input(self): """Test invalid user data.""" self.node.get_logger().info('test_invalid_userdata ...') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - OperatableStateMachine.initialize_ros(self.node) - node = self.node + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class AccessInvalidInputState(EventState): """Local Test state definition.""" def __init__(self): - AccessInvalidInputState.initialize_ros(node) super().__init__(outcomes=['done'], input_keys=['input']) def execute(self, userdata): @@ -160,9 +166,16 @@ def execute(self, userdata): sm = OperatableStateMachine(outcomes=['done']) with sm: - OperatableStateMachine.add('state', AccessInvalidInputState(), transitions={'done': 'done'}) + OperatableStateMachine.add('state', AccessInvalidInputState(), + transitions={'done': 'done'}, + autonomy={'done': 0}) + + outcome = None + try: + outcome = sm.execute(None) + except UserDataError as exc: + self.node.get_logger().info(f" sm had expected exception '{exc}'") - outcome = sm.execute(None) self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, UserDataError) self.node.get_logger().info('test_invalid_userdata - OK! ') @@ -171,15 +184,12 @@ def test_invalid_userdata_output(self): """Test invalid userdata output.""" self.node.get_logger().info('test_invalid_userdata_output ...') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - OperatableStateMachine.initialize_ros(self.node) - node = self.node + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class SetInvalidOutputState(EventState): """Local Test state definition.""" def __init__(self): - SetInvalidOutputState.initialize_ros(node) super().__init__(outcomes=['done'], output_keys=['output']) def execute(self, userdata): @@ -190,7 +200,12 @@ def execute(self, userdata): with sm: OperatableStateMachine.add('state', SetInvalidOutputState(), transitions={'done': 'done'}) - outcome = sm.execute(None) + outcome = None + try: + outcome = sm.execute(None) + except UserDataError as exc: + self.node.get_logger().info(f" sm had expected exception '{exc}'") + self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, UserDataError) self.node.get_logger().info('test_invalid_userdata_output - OK! ') @@ -199,15 +214,12 @@ def test_missing_userdata(self): """Test missing userdata.""" self.node.get_logger().info('test_missing_userdata ...') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - OperatableStateMachine.initialize_ros(self.node) - node = self.node + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class AccessValidInputState(EventState): """Local Test state definition.""" def __init__(self): - AccessValidInputState.initialize_ros(node) super().__init__(outcomes=['done'], input_keys=['missing']) def execute(self, userdata): @@ -218,7 +230,12 @@ def execute(self, userdata): with sm: OperatableStateMachine.add('state', AccessValidInputState(), transitions={'done': 'done'}) - outcome = sm.execute(None) + outcome = None + try: + outcome = sm.execute(None) + except UserDataError as exc: + self.node.get_logger().info(f" sm had expected exception '{exc}'") + self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, UserDataError) self.node.get_logger().info('test_missing_userdata - OK! ') @@ -227,15 +244,12 @@ def test_modify_input_key(self): """Test modify input key.""" self.node.get_logger().info('test_modify_input_key ...! ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - OperatableStateMachine.initialize_ros(self.node) - node = self.node + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class ModifyInputKeyState(EventState): """Local Test state definition.""" def __init__(self): - ModifyInputKeyState.initialize_ros(node) super().__init__(outcomes=['done'], input_keys=['only_input']) def execute(self, userdata): @@ -247,7 +261,12 @@ def execute(self, userdata): with sm: OperatableStateMachine.add('state', ModifyInputKeyState(), transitions={'done': 'done'}) - outcome = sm.execute(None) + outcome = None + try: + outcome = sm.execute(None) + except UserDataError as exc: + self.node.get_logger().info(f" sm had expected exception '{exc}'") + self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, UserDataError) self.node.get_logger().info('test_modify_input_key - OK! ') diff --git a/flexbe_core/test/test_exceptions_spin.py b/flexbe_core/test/test_exceptions_spin.py new file mode 100644 index 0000000..2edd2c9 --- /dev/null +++ b/flexbe_core/test/test_exceptions_spin.py @@ -0,0 +1,267 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +"""Test FlexBE Exception handling with sm.spin().""" +import time +import unittest + +from flexbe_core import EventState, OperatableStateMachine, initialize_flexbe_core +from flexbe_core.core.exceptions import StateError, StateMachineError, UserDataError +from flexbe_core.proxy import shutdown_proxies + +import rclpy +from rclpy.executors import MultiThreadedExecutor + + +class TestExceptionsSpin(unittest.TestCase): + """Test FlexBE Exception handling.""" + + test = 0 + __EXECUTE_TIMEOUT_SEC = 0.2 # 0.025 # Timeout in executor loops for spin once + __TIME_SLEEP = 0.2 # 0.025 # Sleep time for loops + + def __init__(self, *args, **kwargs): + """Initialize TestExceptionsSpin instance.""" + super().__init__(*args, **kwargs) + + def setUp(self): + """Set up the TestExceptionsSpin test.""" + TestExceptionsSpin.test += 1 + self.context = rclpy.context.Context() + rclpy.init(context=self.context) + if not rclpy.ok(context=self.context): + raise RuntimeError('rclpy failed to initialize properly with context') + + self.executor = MultiThreadedExecutor(context=self.context) + self.node = rclpy.create_node('exception_test_' + str(self.test), context=self.context) + self.node.get_logger().info(' set up exceptions test %d (%d) ... ' % (self.test, self.context.ok())) + self.executor.add_node(self.node) + initialize_flexbe_core(self.node) + + def tearDown(self): + """Tear down the TestExceptionsSpin test.""" + self.node.get_logger().info(' shutting down exceptions test %d (%d) ... ' % (self.test, self.context.ok())) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=2 * TestExceptionsSpin.__EXECUTE_TIMEOUT_SEC) + + self.node.get_logger().info(' shutting down proxies in core test %d ... ' % (self.test)) + shutdown_proxies() + time.sleep(TestExceptionsSpin.__TIME_SLEEP) + + self.node.get_logger().info(' destroy node in core test %d ... ' % (self.test)) + self.node.destroy_node() + + time.sleep(TestExceptionsSpin.__TIME_SLEEP) + self.executor.shutdown() + time.sleep(TestExceptionsSpin.__TIME_SLEEP) + + # Kill it with fire to make sure not stray published topics are available + rclpy.shutdown(context=self.context) + time.sleep(TestExceptionsSpin.__TIME_SLEEP * 2) + + def test_invalid_outcome(self): + """Test invalid outcome.""" + self.node.get_logger().info('test_invalid_outcome ...') + + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptionsSpin.__EXECUTE_TIMEOUT_SEC) + + class ReturnInvalidOutcomeState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done']) + self._state_id = 256 + + def execute(self, userdata): + self._node.get_logger().info('test_invalid_outcome - execute with invalid outcome ...') + return 'invalid' + + def on_enter(self, userdata): + self._node.get_logger().info('test_invalid_outcome - on_enter ...') + super().on_enter(userdata) + + sm = OperatableStateMachine(name='test_invalid_outcome', outcomes=['done']) + sm._state_id = 1024 + + with sm: + OperatableStateMachine.add('state', ReturnInvalidOutcomeState(), transitions={'done': 'done'}) + outcome = sm.spin(None, rclpy_context=self.context) + + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, StateError) + self.node.get_logger().info('test_invalid_outcome - OK! ') + + def test_invalid_transition(self): + """Test invalid transition.""" + self.node.get_logger().info('test_invalid_transition ...') + + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptionsSpin.__EXECUTE_TIMEOUT_SEC) + + class ReturnDoneState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done']) + self._state_id = 256 + + def execute(self, userdata): + return 'done' + + inner_sm = OperatableStateMachine(outcomes=['done']) + inner_sm._state_id = 512 + with inner_sm: + OperatableStateMachine.add('state', ReturnDoneState(), transitions={'done': 'invalid'}) + sm = OperatableStateMachine(name='test_invalid_transition', outcomes=['done']) + sm._state_id = 1024 + with sm: + OperatableStateMachine.add('inner', inner_sm, transitions={'done': 'done'}) + + outcome = sm.spin(None, rclpy_context=self.context) + + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, StateMachineError) + self.node.get_logger().info('test_invalid_transition - OK! ') + + def test_invalid_userdata_input(self): + """Test invalid user data.""" + self.node.get_logger().info('test_invalid_userdata ...') + + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptionsSpin.__EXECUTE_TIMEOUT_SEC) + + class AccessInvalidInputState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done'], input_keys=['input']) + self._state_id = 256 + + def execute(self, userdata): + print(userdata.invalid) + return 'done' + + sm = OperatableStateMachine(name='test_invalid_userdata_input', outcomes=['done']) + sm._state_id = 1023 + with sm: + OperatableStateMachine.add('state', AccessInvalidInputState(), transitions={'done': 'done'}) + + outcome = sm.spin(None, rclpy_context=self.context) + + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, UserDataError) + self.node.get_logger().info('test_invalid_userdata - OK! ') + + def test_invalid_userdata_output(self): + """Test invalid userdata output.""" + self.node.get_logger().info('test_invalid_userdata_output ...') + + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptionsSpin.__EXECUTE_TIMEOUT_SEC) + + class SetInvalidOutputState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done'], output_keys=['output']) + self._state_id = 256 + + def execute(self, userdata): + userdata.invalid = False + return 'done' + + sm = OperatableStateMachine(name='test_invalid_userdata_output', outcomes=['done']) + sm._state_id = 1024 + with sm: + OperatableStateMachine.add('state', SetInvalidOutputState(), transitions={'done': 'done'}) + + outcome = sm.spin(None, rclpy_context=self.context) + + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, UserDataError) + self.node.get_logger().info('test_invalid_userdata_output - OK! ') + + def test_missing_userdata(self): + """Test missing userdata.""" + self.node.get_logger().info('test_missing_userdata ...') + + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptionsSpin.__EXECUTE_TIMEOUT_SEC) + + class AccessValidInputState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done'], input_keys=['missing']) + self._state_id = 256 + + def execute(self, userdata): + print(userdata.missing) + return 'done' + + sm = OperatableStateMachine(name='test_missing_userdata', outcomes=['done']) + sm._state_id = 1024 + with sm: + OperatableStateMachine.add('state', AccessValidInputState(), transitions={'done': 'done'}) + + outcome = sm.spin(None, rclpy_context=self.context) + + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, UserDataError) + self.node.get_logger().info('test_missing_userdata - OK! ') + + def test_modify_input_key(self): + """Test modify input key.""" + self.node.get_logger().info('test_modify_input_key ...! ') + + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptionsSpin.__EXECUTE_TIMEOUT_SEC) + + class ModifyInputKeyState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done'], input_keys=['only_input']) + self._state_id = 256 + + def execute(self, userdata): + userdata.only_input['new'] = 'not_allowed' + return 'done' + + sm = OperatableStateMachine(name='test_modify_input_key', outcomes=['done']) + sm._state_id = 1024 + sm.userdata.only_input = {'existing': 'is_allowed'} + with sm: + OperatableStateMachine.add('state', ModifyInputKeyState(), transitions={'done': 'done'}) + + outcome = sm.spin(None, rclpy_context=self.context) + + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, UserDataError) + self.node.get_logger().info('test_modify_input_key - OK! ') + + +if __name__ == '__main__': + unittest.main() diff --git a/flexbe_core/test/test_logger.py b/flexbe_core/test/test_logger.py index 225cf99..f9a4da6 100644 --- a/flexbe_core/test/test_logger.py +++ b/flexbe_core/test/test_logger.py @@ -33,9 +33,9 @@ import time import unittest -from flexbe_core import EventState, OperatableStateMachine, set_node +from flexbe_core import EventState, OperatableStateMachine, initialize_flexbe_core from flexbe_core.logger import Logger -from flexbe_core.proxy import initialize_proxies, shutdown_proxies +from flexbe_core.proxy import shutdown_proxies import rclpy from rclpy.executors import MultiThreadedExecutor @@ -45,6 +45,8 @@ class TestLogger(unittest.TestCase): """Test FlexBE Logger handling.""" test = 0 + __EXECUTE_TIMEOUT_SEC = 0.2 # 0.025 # Timeout in executor loops for spin once + __TIME_SLEEP = 0.2 # 0.025 # Sleep time for loops def __init__(self, *args, **kwargs): """Initialize TestLogger instance.""" @@ -55,49 +57,45 @@ def setUp(self): TestLogger.test += 1 self.context = rclpy.context.Context() rclpy.init(context=self.context) - self.executor = MultiThreadedExecutor(context=self.context) self.node = rclpy.create_node('logger_test_' + str(self.test), context=self.context) self.node.get_logger().info(' set up logger test %d (%d) ... ' % (self.test, self.context.ok())) self.executor.add_node(self.node) - initialize_proxies(self.node) + initialize_flexbe_core(self.node) def tearDown(self): """Tear down the test.""" self.node.get_logger().info(' shutting down logger test %d (%d) ... ' % (self.test, self.context.ok())) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestLogger.__EXECUTE_TIMEOUT_SEC) self.node.get_logger().info(' shutting down proxies in logger test %d ... ' % (self.test)) shutdown_proxies() - time.sleep(0.1) + time.sleep(TestLogger.__TIME_SLEEP) self.node.get_logger().info(' destroy node in core test %d ... ' % (self.test)) self.node.destroy_node() - time.sleep(0.1) + time.sleep(TestLogger.__TIME_SLEEP) self.executor.shutdown() - time.sleep(0.1) + time.sleep(TestLogger.__TIME_SLEEP) # Kill it with fire to make sure not stray published topics are available rclpy.shutdown(context=self.context) - time.sleep(0.2) + time.sleep(TestLogger.__TIME_SLEEP * 2) def test_throttle_logger_one(self): """Test throttle logger one.""" self.node.get_logger().info('test_throttle_logger_one ...') self.node.declare_parameter('max_throttle_logging_size', 100) self.node.declare_parameter('throttle_logging_clear_ratio', 0.25) - set_node(self.node) # Update the logger node + initialize_flexbe_core(self.node) # Update the logger node - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - OperatableStateMachine.initialize_ros(self.node) - node = self.node + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestLogger.__EXECUTE_TIMEOUT_SEC) class ThrottleSingleLog(EventState): """Local Test state definition.""" def __init__(self): - self.initialize_ros(node) super().__init__(outcomes=['done']) self._trials = Logger.MAX_LAST_LOGGED_SIZE * 2 Logger.logerr_throttle(0.0, 'test') @@ -126,21 +124,18 @@ def execute(self, userdata): self.node.get_logger().info('test_throttle_logger_one - OK! ') def test_throttle_logger_err_multi(self): - """Test throttle logger with errrors.""" + """Test throttle logger with errors.""" self.node.get_logger().info('test_throttle_logger_err_multi ...') self.node.declare_parameter('max_throttle_logging_size', 200) self.node.declare_parameter('throttle_logging_clear_ratio', 0.35) - set_node(self.node) # Update the logger node + initialize_flexbe_core(self.node) # Update the logger node - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - OperatableStateMachine.initialize_ros(self.node) - node = self.node + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestLogger.__EXECUTE_TIMEOUT_SEC) class ThrottleMultiLog(EventState): """Local Test state definition.""" def __init__(self): - self.initialize_ros(node) super().__init__(outcomes=['done']) self._trials = Logger.MAX_LAST_LOGGED_SIZE * 2 Logger.logerr_throttle(0.01, '0_test') @@ -174,17 +169,14 @@ def test_throttle_logger_multiple_params(self): self.node.declare_parameter('max_throttle_logging_size', 100) self.node.declare_parameter('throttle_logging_clear_ratio', 0.7) - set_node(self.node) # Update the logger node + initialize_flexbe_core(self.node) # Update the logger node - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - OperatableStateMachine.initialize_ros(self.node) - node = self.node + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestLogger.__EXECUTE_TIMEOUT_SEC) class ThrottleMultiLog(EventState): """Local Test state definition.""" def __init__(self): - self.initialize_ros(node) super().__init__(outcomes=['done']) self._trials = Logger.MAX_LAST_LOGGED_SIZE * 2 Logger.logerr_throttle(0.01, '0_test') @@ -210,7 +202,7 @@ def execute(self, userdata): while outcome is None: outcome = sm.execute(None) self.assertTrue(1 < len(Logger._last_logged) <= Logger.MAX_LAST_LOGGED_SIZE) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.001) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestLogger.__EXECUTE_TIMEOUT_SEC * 0.5) self.assertEqual(outcome, 'done') self.assertEqual(state_instance._trials, 0) @@ -222,17 +214,14 @@ def test_throttle_logger_multiple(self): self.node.get_logger().info('test_throttle_logger_multiple_params ...') self.node.declare_parameter('max_throttle_logging_size', 120) self.node.declare_parameter('throttle_logging_clear_ratio', 0.22) - set_node(self.node) # Update the logger node + initialize_flexbe_core(self.node) # Update the logger node - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - OperatableStateMachine.initialize_ros(self.node) - node = self.node + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestLogger.__EXECUTE_TIMEOUT_SEC) class ThrottleMultiLog(EventState): """Local Test state definition.""" def __init__(self): - self.initialize_ros(node) super().__init__(outcomes=['done']) self._trials = Logger.MAX_LAST_LOGGED_SIZE * 2 Logger.logerr_throttle(0.01, '0_test') @@ -258,7 +247,7 @@ def execute(self, userdata): while outcome is None: outcome = sm.execute(None) self.assertTrue(1 < len(Logger._last_logged) <= Logger.MAX_LAST_LOGGED_SIZE) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.001) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestLogger.__EXECUTE_TIMEOUT_SEC * 0.5) self.assertEqual(outcome, 'done') self.assertEqual(state_instance._trials, 0) diff --git a/flexbe_core/test/test_proxies.py b/flexbe_core/test/test_proxies.py index 0482a32..b200bb6 100755 --- a/flexbe_core/test/test_proxies.py +++ b/flexbe_core/test/test_proxies.py @@ -54,6 +54,8 @@ class TestProxies(unittest.TestCase): """Test the FlexBE proxies.""" test = 0 + __EXECUTE_TIMEOUT_SEC = 0.2 # 0.025 # Timeout in executor loops for spin once + __TIME_SLEEP = 0.2 # 0.025 # Sleep time for loops def __init__(self, *args, **kwargs): """Initialize TestProxies instance.""" @@ -76,28 +78,28 @@ def setUp(self): def tearDown(self): """Tear down the test.""" self.node.get_logger().info(' shutting down proxies test %d (%d) ... ' % (self.test, self.context.ok())) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) self.node.get_logger().info(' shutting down proxies in core test %d ... ' % (self.test)) shutdown_proxies() - time.sleep(0.1) + time.sleep(TestProxies.__TIME_SLEEP) self.node.get_logger().info(' destroy node in core test %d ... ' % (self.test)) self.node.destroy_node() - time.sleep(0.1) + time.sleep(TestProxies.__TIME_SLEEP) self.executor.shutdown() - time.sleep(0.1) + time.sleep(TestProxies.__TIME_SLEEP) # Kill it with fire to make sure not stray published topics are available rclpy.shutdown(context=self.context) - time.sleep(0.5) + time.sleep(TestProxies.__TIME_SLEEP * 5) def test_publish_subscribe(self): """Test publish and subscribe.""" self.node.get_logger().info('test_publish_subscribe ...') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) ProxyPublisher.initialize(self.node) ProxySubscriberCached.initialize(self.node) @@ -106,23 +108,23 @@ def test_publish_subscribe(self): self.node.get_logger().info('test_publish_subscribe - define publishers ...') pub = ProxyPublisher({topic1: String}) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) pub = ProxyPublisher({topic2: String}) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) self.node.get_logger().info(' subscribe topic1 only ...') sub = ProxySubscriberCached({topic1: String}, inst_id=id(self)) - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) self.assertTrue(pub.is_available(topic1)) # cannot call wait given spin_once structure # self.assertTrue(pub.wait_for_any(topic1)) # self.assertFalse(pub.wait_for_any(topic2)) for _ in range(50): - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.01) - time.sleep(0.04) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) + time.sleep(TestProxies.__TIME_SLEEP) self.assertTrue(pub.number_of_subscribers(topic1) > 0) self.assertFalse(pub.number_of_subscribers(topic2) > 0) @@ -134,8 +136,8 @@ def test_publish_subscribe(self): self.node.get_logger().info(' subscribe topic2 ...') sub = ProxySubscriberCached({topic2: String}, inst_id=id(self)) for _ in range(50): - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.01) - time.sleep(0.05) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) + time.sleep(TestProxies.__TIME_SLEEP) self.assertTrue(pub.number_of_subscribers(topic1) > 0) self.assertTrue(pub.number_of_subscribers(topic2) > 0) @@ -157,7 +159,7 @@ def test_publish_subscribe(self): self.node.get_logger().info(' listen for two messages ...') end_time = time.time() + 5 while time.time() < end_time: - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) self.assertTrue(sub.has_msg(topic1)) self.assertEqual(sub.get_last_msg(topic1).data, '1') @@ -186,8 +188,8 @@ def test_subscribe_buffer(self): sub.enable_buffer(topic1) # No wait in this setup - self.assertTrue(pub.wait_for_any(topic1)) for _ in range(10): - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.01) - time.sleep(0.05) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) + time.sleep(TestProxies.__TIME_SLEEP) self.assertTrue(pub.number_of_subscribers(topic1) > 0) @@ -202,7 +204,7 @@ def test_subscribe_buffer(self): # make sure messages can be received end_time = time.time() + 3 while time.time() < end_time: - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) self.assertTrue(sub.has_msg(topic1)) self.assertTrue(sub.has_buffered(topic1)) @@ -215,7 +217,7 @@ def test_subscribe_buffer(self): # make sure message can be received end_time = time.time() + 3 while time.time() < end_time: - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) self.assertEqual(sub.get_from_buffer(topic1).data, '2') self.assertEqual(sub.get_from_buffer(topic1).data, '3') @@ -244,7 +246,7 @@ def server_callback(request, response): srv.call_async(topic1, Trigger.Request()) end_time = time.time() + 10 while time.time() < end_time: - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) self.assertTrue(srv.done(topic1)) @@ -265,7 +267,7 @@ def test_action_client(self): topic1 = '/action_1' def execute_cb(goal_handle): - time.sleep(0.1) + time.sleep(TestProxies.__TIME_SLEEP) if goal_handle.is_cancel_requested: goal_handle.canceled() return BehaviorExecution.Result() @@ -291,7 +293,7 @@ def execute_cb(goal_handle): while time.time() < end_time and not client.has_result(topic1): status = client.get_status(topic1) # self.node.get_logger().info(f' get status = {status} ') - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) self.assertTrue(client.is_active(topic1) or client.has_result(topic1)) self.assertTrue(client.has_result(topic1)) @@ -315,7 +317,7 @@ def execute_cb(goal_handle): # end_time = time.time() + 2 while not client.has_result(topic1): - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestProxies.__EXECUTE_TIMEOUT_SEC) self.assertTrue(client.is_active(topic1) or client.has_result(topic1)) status = client.get_status(topic1) # self.node.get_logger().info(f' check status = {status} ') @@ -333,7 +335,9 @@ def execute_cb(goal_handle): self.assertTrue(client.has_result(topic1)) client.remove_result(topic1) self.assertIsNone(client._result.get(topic1)) - self.assertIsNone(client._result_status.get(topic1)) + self.assertEqual(status, GoalStatus.STATUS_SUCCEEDED) + # -- we are now preserving status until cleared on next goal + # self.assertIsNone(client._result_status.get(topic1)) self.assertFalse(client.has_result(topic1)) self.node.get_logger().info('validated remove_result! ') @@ -341,7 +345,7 @@ def execute_cb(goal_handle): client = ProxyActionClient({'/invalid': BehaviorExecution}, wait_duration=.1) self.assertFalse(client.is_available('/invalid')) self.node.get_logger().info('test_action_client - OK! ') - del server # Through with instance, and explicitly calling del() to avoid ununsed warning + del server # Through with instance, and explicitly calling del() to avoid unused warning if __name__ == '__main__': diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index 7abb614..bc72937 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.0.1 (2024-09-26) +------------------ +* codespell clean up +* fix typo in input_action_server + +4.0.0 (2024-08-24) +------------------ +* Version 4.0.0 release using state_id for communication +* this breaks API with flexbe_app and requires version 4.1.0+ of the FlexBE WebUI API + +3.0.7 (2024-08-24) +------------------ +* fix issue with input action server +* add new states; modify BehaviorInput to allow strings and selection combo box +* add initialize_flexbe_core for common initialization + 3.0.6 (2024-08-05) ------------------ * allow canceling input request diff --git a/flexbe_input/flexbe_input/complex_action_server.py b/flexbe_input/flexbe_input/complex_action_server.py index c2173aa..de3628a 100644 --- a/flexbe_input/flexbe_input/complex_action_server.py +++ b/flexbe_input/flexbe_input/complex_action_server.py @@ -63,7 +63,7 @@ class ComplexActionServer: @param execute_cb Optional callback that gets called in a separate thread whenever a new goal is received, allowing users to have blocking callbacks. Adding an execute callback also deactivates the goalCallback. - @param auto_start A boolean value that tells the ActionServer wheteher or not + @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. @@ -196,7 +196,7 @@ def internal_goal_callback(self, goal): self.execute_condition.acquire() try: - Logger.localinfo(f'A new goal {goal.goal_id} has been recieved by the single goal action server') + Logger.localinfo(f'A new goal {goal.goal_id} has been received by the single goal action server') self.next_goal = goal self.new_goal = True @@ -220,7 +220,7 @@ def internal_preempt_callback(self, preempt): # @brief Called from a separate thread to call blocking execute calls def executeLoop(self): - """Excute the server loop.""" + """Execute the server loop.""" loop_duration = Duration(seconds=0.1) while (rclpy.ok()): diff --git a/flexbe_input/flexbe_input/input_action_server.py b/flexbe_input/flexbe_input/input_action_server.py index 5f3fdfb..2f0a170 100644 --- a/flexbe_input/flexbe_input/input_action_server.py +++ b/flexbe_input/flexbe_input/input_action_server.py @@ -30,7 +30,7 @@ import pickle import time -from PySide6.QtCore import Signal, Slot, QCoreApplication, Qt, QThread +from PySide6.QtCore import QCoreApplication, QThread, Qt, Signal, Slot from PySide6.QtWidgets import QApplication from flexbe_core import Logger @@ -48,7 +48,9 @@ class InputActionWorker(QThread): - _show_dialog_signal = Signal(str) + """Worker thread for InputAction server.""" + + _show_dialog_signal = Signal(str, object) _hide_dialog_signal = Signal() def __init__(self, node): @@ -56,10 +58,11 @@ def __init__(self, node): self._node = node def run(self): + """Run loop for worker thread.""" try: # Use a MultiThreadedExecutor to enable processing goals concurrently executor = MultiThreadedExecutor() - print('Begin spining ROS loop for InputActionServer ...', flush=True) + print('Begin spinning ROS loop for InputActionServer ...', flush=True) rclpy.spin(self._node, executor=executor) except (KeyboardInterrupt, ExternalShutdownException): print('Caught KeyboardInterrupt in InputActionWorker thread - shutdown ...') @@ -91,7 +94,7 @@ def __init__(self): cancel_callback=self.cancel_callback ) - self._input_dialog = InputGUI("default") + self._input_dialog = InputGUI('default') self._input = None self._canceled = False @@ -107,12 +110,14 @@ def get_input_type(self, request_type): @return prompt, instance type, number of elements """ - # Thse are the only types handled by this simple UI + # These are the only types handled by this simple UI types = {BehaviorInput.Goal.REQUEST_INT: ('int', int, 1), BehaviorInput.Goal.REQUEST_FLOAT: ('float', (float, int), 1), # int acceptable for desired float BehaviorInput.Goal.REQUEST_2D: ('list of 2 numbers', (list, tuple), 2), # allow either list or tuple BehaviorInput.Goal.REQUEST_3D: ('list of 3 numbers', (list, tuple), 3), # e.g., '[1, 2]', '(1, 2)', or '1, 2' BehaviorInput.Goal.REQUEST_4D: ('list of 4 numbers', (list, tuple), 4), + BehaviorInput.Goal.REQUEST_STRING: ('string', str, 1), + BehaviorInput.Goal.REQUEST_SELECTION: ('selected item', str, 1) } if request_type in types: @@ -141,7 +146,10 @@ def execute_callback(self, goal_handle): # Get data from user try: # Request input from GUI running in a separate thread - self._worker._show_dialog_signal.emit(prompt_text) + if goal_handle.request.request_type == BehaviorInput.Goal.REQUEST_SELECTION: + self._worker._show_dialog_signal.emit(prompt_text, goal_handle.request.items) + else: + self._worker._show_dialog_signal.emit(prompt_text, None) while self._input_dialog.is_none() and not self._canceled: time.sleep(0.02) # Add a short sleep to avoid busy-waiting @@ -163,15 +171,25 @@ def execute_callback(self, goal_handle): goal_handle.abort() return result else: - input_data = ast.literal_eval(self._input) # convert string to Python data - if not isinstance(input_data, type_class): - result.data = f"Invalid input type '{type(input_data)}' not '{type_class}' - expected '{type_text}'" - result.result_code = BehaviorInput.Result.RESULT_FAILED - Logger.localwarn(result.data) - goal_handle.abort() - return result + if type_class is str: + print(f"Process data as string '{self._input}' with request {type_class}", flush=True) + result.data = self._input + data_len = 1 + else: + print(f"Process data '{self._input}' as {type_class}", flush=True) + input_data = ast.literal_eval(self._input) # convert string to Python data + print(f" input data[{type(input_data)}] = '{input_data}'", flush=True) + data_len = 1 if isinstance(input_data, (int, float)) else len(input_data) + + if not isinstance(input_data, type_class): + result.data = f"Invalid input type '{type(result.data)}' not '{type_class}' - expected '{type_text}'" + result.result_code = BehaviorInput.Result.RESULT_FAILED + Logger.localwarn(result.data) + goal_handle.abort() + return result + # Convert binary to string for transport + result.data = str(pickle.dumps(input_data)) - data_len = 1 if isinstance(input_data, (int, float)) else len(input_data) if data_len != expected_elements: result.data = (f'Invalid number of elements {data_len} not {expected_elements} ' f"of {type_class} - expected '{type_text}'") @@ -179,7 +197,6 @@ def execute_callback(self, goal_handle): Logger.localwarn(result.data) goal_handle.abort() else: - result.data = str(pickle.dumps(input_data)) result.result_code = BehaviorInput.Result.RESULT_OK goal_handle.succeed() except Exception as exc: # pylint: disable=W0703 @@ -192,12 +209,14 @@ def execute_callback(self, goal_handle): return result def cancel_callback(self, goal_handle): + """Cancel the active goal.""" Logger.localwarn(f"Canceling goal for '{self._action_topic}' ...") self._canceled = True return rclpy.action.CancelResponse.ACCEPT @Slot() - def on_get_input(self, val=0): + def on_get_input(self): + """Get the input from edit box.""" self._input = self._input_dialog.get_input() @@ -228,9 +247,9 @@ def main(args=[]): print('Ensure shutdown of ROS worker thread ...', flush=True) worker.quit() - print("wait on ROS thread to close ...", flush=True) + print('wait on ROS thread to close ...', flush=True) worker.wait() - print("done!", flush=True) + print('done!', flush=True) if __name__ == '__main__': diff --git a/flexbe_input/flexbe_input/input_gui.py b/flexbe_input/flexbe_input/input_gui.py index 058de20..5a5650d 100644 --- a/flexbe_input/flexbe_input/input_gui.py +++ b/flexbe_input/flexbe_input/input_gui.py @@ -28,7 +28,7 @@ """FlexBE InputGUI.""" from PySide6.QtCore import QSize, Slot -from PySide6.QtWidgets import QLabel, QLineEdit, QMainWindow, QPushButton, QVBoxLayout, QWidget +from PySide6.QtWidgets import QComboBox, QLabel, QLineEdit, QMainWindow, QPushButton, QVBoxLayout, QWidget class InputGUI(QMainWindow): @@ -53,34 +53,96 @@ def __init__(self, prompt): } """) - central_widget = QWidget(self) - self.setCentralWidget(central_widget) - central_widget.setStyleSheet("QWidget { border: 1px solid blue; background-color: palette(window); }") - - layout = QVBoxLayout(central_widget) + self.central_widget = QWidget(self) + self.setCentralWidget(self.central_widget) + self.main_layout = QVBoxLayout(self.central_widget) + self.central_widget.setStyleSheet('QWidget { border: 1px solid blue; background-color: palette(window); }') + self.combo_box = None + self.input_line = None + + def clear_layout(self): + """Clear prior layout of widget.""" + while self.main_layout.count(): + item = self.main_layout.takeAt(0) + widget = item.widget() + if widget is not None: + widget.setParent(None) # Detach the widget from the layout + widget.deleteLater() + + def set_layout(self, prompt, items=None): + """Update the widget layout.""" + self.clear_layout() self.prompt = QLabel(self) + self.prompt.setStyleSheet('QLabel { border: none; background-color: palette(window); }') self.prompt.setText(prompt) - self.prompt.setStyleSheet("QLabel { border: none; background-color: palette(window); }") - layout.addWidget(self.prompt) + self.prompt.adjustSize() + self.main_layout.addWidget(self.prompt) - edit_style = """ - QLineEdit { - border: 2px solid #8f8f91; - background-color: #f0f0f0; - padding: 2px; - color: black; - } + if items is None: + # One line entry field + edit_style = """ + QLineEdit { + border: 2px solid #8f8f91; + background-color: #f0f0f0; + padding: 2px; + color: black; + } - QLineEdit:focus { - border: 2px solid #0078d7; /* Change this color to your desired highlight color */ - } - """ + QLineEdit:focus { + border: 2px solid #0078d7; /* Change this color to your desired highlight color */ + } + """ + + self.input_line = QLineEdit(self) + self.input_line.setStyleSheet(edit_style) + self.input_line.returnPressed.connect(self.set_input) # Treat return as submit + self.input_line.setText('') + self.combo_box = None + self.main_layout.addWidget(self.input_line) + else: + combo_style = """ + QComboBox { + border: 2px solid #8f8f91; + background-color: #f0f0f0; + padding: 2px; + color: black; + } + + QComboBox:focus { + border: 2px solid #0078d7; /* Highlight color */ + } + + QComboBox::drop-down { + border-left: 1px solid #8f8f91; + } + + QComboBox::down-arrow { + image: url(down_arrow.png); /* Optionally customize the down arrow */ + width: 10px; + height: 10px; + } + + QComboBox QAbstractItemView { + background-color: #f0f0f0; + selection-background-color: #0078d7; + border: 1px solid #8f8f91; + } + """ + + self.combo_box = QComboBox(self) + self.combo_box.setStyleSheet(combo_style) + string_items = [] + for item in items: + if not isinstance(item, str): + string_items.append(f'{item}') # Convert to string if not already + else: + string_items.append(item) + self.combo_box.addItems(string_items) + self.combo_box.currentIndexChanged.connect(self.change_selection) + self.line = None + self.main_layout.addWidget(self.combo_box) - self.line = QLineEdit(self) - self.line.setStyleSheet(edit_style) - self.line.returnPressed.connect(self.set_input) # Treat return as submit - layout.addWidget(self.line) button_style = """ QPushButton { border: 2px solid #8f8f91; @@ -100,38 +162,62 @@ def __init__(self, prompt): """ self.submit = QPushButton('Submit', self) self.submit.setStyleSheet(button_style) - self.submit.clicked.connect(self.set_input) - layout.addWidget(self.submit) + if items is None: + self.submit.clicked.connect(self.set_input) + else: + self.submit.clicked.connect(self.set_selection) + + self.main_layout.addWidget(self.submit) self.cancel = QPushButton('Cancel', self) self.cancel.setStyleSheet(button_style) self.cancel.clicked.connect(self.set_cancel) - layout.addWidget(self.cancel) + self.main_layout.addWidget(self.cancel) self.adjustSize() + def change_selection(self): + """Print selection.""" + if self.combo_box is None: + print('Unknown combo box - why is this called!', flush=True) + else: + print(f" Currently selected '{self.combo_box.currentText()}' ") + + def set_selection(self): + """Set input text from selection box.""" + if self.combo_box is None: + print('Unknown combo box - why is this called!', flush=True) + self.input = 'unknown' + else: + print(f" Selected '{self.combo_box.currentText()}' ") + self.input = self.combo_box.currentText() + def set_input(self): """Set input text from GUI.""" - self.input = self.line.text() + if self.input_line is None: + print('Unknown combo box - why is this called!', flush=True) + self.input = 'unknown' + else: + self.input = self.input_line.text() def set_cancel(self): """Set input text from GUI.""" self.input = '' @Slot(str) - def show(self, prompt): - print(f"showing dialog with '{prompt}' ", flush=True) - self.prompt.setText(prompt) - self.prompt.adjustSize() - self.line.setText("") + def show(self, prompt, items=None): + """Show dialog if hidden.""" + print(f"showing input UI dialog with '{prompt}' ", flush=True) self.input = None # clear for next entry - self.adjustSize() + self.set_layout(prompt, items) self.resize(self.sizeHint()) # Resize to fit the new content super().show() @Slot() def hide(self): - print("hiding dialog", flush=True) + """Hide dialog when not in use.""" + print('hiding input UI dialog', flush=True) + self.clear_layout() super().hide() def is_none(self): diff --git a/flexbe_input/package.xml b/flexbe_input/package.xml index 91a882b..2815c04 100644 --- a/flexbe_input/package.xml +++ b/flexbe_input/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> flexbe_input - 3.0.6 + 4.0.1 flexbe_input enables to send data to onboard behavior when required. diff --git a/flexbe_input/setup.py b/flexbe_input/setup.py index 5560410..85d9e6d 100644 --- a/flexbe_input/setup.py +++ b/flexbe_input/setup.py @@ -6,7 +6,7 @@ setup( name=PACKAGE_NAME, - version='3.0.6', + version='4.0.1', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]), diff --git a/flexbe_mirror/CHANGELOG.rst b/flexbe_mirror/CHANGELOG.rst index d9b36ac..5046fb5 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.0.1 (2024-09-26) +------------------ +* codespell clean up +* report all states in deep_states including containers and finished states +* re-request outcome on sync request +* notify skipped for containers + +4.0.0 (2024-08-24) +------------------ +* Version 4.0.0 release using state_id for communication +* this breaks API with flexbe_app and requires version 4.1.0+ of the FlexBE WebUI API +* use state id consistently to avoid long path strings +* modify preempt and published outcome to improve sync +* add flexbe_outcome_listener node for simple monitoring + +3.0.7 (2024-08-24) +------------------ +* modify mirror handling for controllable OSM/CC and improve sync +* add initialize_flexbe_core for common initialization +* updates to ConcurrencyContainer and StateMachine to handle sync and forced outcomes + 3.0.6 (2024-08-05) ------------------ * clean up sync messaging and set entering flag for state diff --git a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py index b88ff28..f503f1d 100644 --- a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py @@ -42,13 +42,13 @@ def set_thread_name(name): # print('Python thread names are not visible in ps/top unless you install prctl') pass -from flexbe_core import Logger, MIN_UI_VERSION +from flexbe_core import Logger, MIN_UI_VERSION, initialize_flexbe_core from flexbe_core.core import LockableStateMachine, OperatableStateMachine -from flexbe_core.core import PreemptableState, PreemptableStateMachine, StateMap +from flexbe_core.core import PreemptableState, State, StateMap from flexbe_core.core.topics import Topics from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached -from flexbe_msgs.msg import BEStatus, BehaviorSync, ContainerStructure +from flexbe_msgs.msg import BEStatus, BehaviorSync, ContainerStructure, StateMapMsg from rclpy.clock import Clock from rclpy.node import Node @@ -82,19 +82,12 @@ def __init__(self): super().__init__('flexbe_mirror') self._sm = None - ProxyPublisher.initialize(self) - ProxySubscriberCached.initialize(self) - Logger.initialize(self) - - MirrorState.initialize_ros(self) - PreemptableState.initialize_ros(self) - PreemptableStateMachine.initialize_ros(self) - LockableStateMachine.initialize_ros(self) + initialize_flexbe_core(self) self._timing_event = threading.Event() # Used for wait timer # Keep track of mirror thread status - # starting one while other is stoppin is valid, + # starting one while other is stopping is valid, # but only one thread should be running at a time self._starting = False self._running = False @@ -110,7 +103,7 @@ def __init__(self): self._system_clock = Clock() self._active_thread_start = None - # set up proxys for sm <--> GUI communication + # set up proxies for sm <--> GUI communication # publish topics self._heartbeat_pub = self.create_publisher(Int32, Topics._MIRROR_HEARTBEAT_TOPIC, 2) latching_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) @@ -130,12 +123,14 @@ def __init__(self): # Use proxy publisher/subscriber for access in states # but just initialize here once for all - self._beh_update_pub = ProxyPublisher({Topics._BEHAVIOR_UPDATE_TOPIC: String}) + self._beh_update_pub = ProxyPublisher({Topics._BEHAVIOR_UPDATE_TOPIC: Int32}) self._outcome_sub = ProxySubscriberCached() self._outcome_sub.subscribe(Topics._OUTCOME_TOPIC, UInt32, inst_id=id(self)) self._outcome_sub.enable_buffer(Topics._OUTCOME_TOPIC) + self._state_map_pub = self.create_publisher(StateMapMsg, Topics._STATE_MAP_OCS_TOPIC, latching_qos) + # no clean way to wait for publisher to be ready... Logger.loginfo('--> Mirror - setting up publishers and subscribers ...') threading.Event().wait(1.0) # Give publishers time to initialize @@ -150,10 +145,10 @@ def _version_callback(self, msg): vui = FlexbeMirror._parse_version(msg.data) vex = FlexbeMirror._parse_version(MIN_UI_VERSION) if vui < vex: - Logger.logwarn('FlexBE App needs to be updated!\n' + Logger.logwarn('FlexBE WebUI needs to be updated!\n' f'Mirror requires at least version {MIN_UI_VERSION}, ' f' but you have {msg.data}\n' - 'Please update the flexbe_app software.') + 'Please update the flexbe_webui software.') @staticmethod def _parse_version(v): @@ -162,7 +157,7 @@ def _parse_version(v): offset = 1 for n in reversed(v.split('.')): result += int(n) * offset - offset *= 100 + offset *= 1000 return result def get_elapsed_str(self, start_time): @@ -282,7 +277,7 @@ def _activate_mirror(self, struct_msg, start_time): except Exception as exc: # pylint: disable=W0703 Logger.logerr(f'Exception in activate mirror: {type(exc)} started at {start_time.nanoseconds} ns ...\n {exc}') Logger.localerr(f"{traceback.format_exc().replace('%', '%%')}") - self._running = False # normally set false in execute_mirror (but not if exeception) + self._running = False # normally set false in execute_mirror (but not if exception) Logger.localwarn(f'Done executing mirror {self._active_id} from activation ' f'{self.get_elapsed_str(start_time)}') @@ -424,16 +419,16 @@ def _stop_mirror(self, msg, start_time): Logger.logwarn('Onboard behavior stop request (from sync)!') elif msg.code == BEStatus.FINISHED: Logger.loginfo('Onboard behavior finished successfully.') - self._beh_update_pub.publish(Topics._BEHAVIOR_UPDATE_TOPIC, String()) + self._beh_update_pub.publish(Topics._BEHAVIOR_UPDATE_TOPIC, Int32(data=-1)) elif msg.code == BEStatus.SWITCHING: self._starting_path = None Logger.loginfo('Onboard performing behavior switch.') elif msg.code == BEStatus.READY: Logger.loginfo('Onboard engine just started, stopping currently running mirror.') - self._beh_update_pub.publish(Topics._BEHAVIOR_UPDATE_TOPIC, String()) + self._beh_update_pub.publish(Topics._BEHAVIOR_UPDATE_TOPIC, Int32(data=-1)) else: Logger.logwarn('Onboard behavior failed!') - self._beh_update_pub.publish(Topics._BEHAVIOR_UPDATE_TOPIC, String()) + self._beh_update_pub.publish(Topics._BEHAVIOR_UPDATE_TOPIC, Int32(data=-1)) self._wait_stop_running(start_time) @@ -460,6 +455,12 @@ def _sync_callback(self, msg): thread = threading.Thread(target=self._restart_mirror, args=[msg, start_time]) thread.daemon = True thread.start() + + # Force a new update after sync + Logger.localinfo('\x1b[93mReceived sync for current behavior - request behavior update message\x1b[0m') + MirrorStateMachine._execute_flag = True # Execute once more after any change, + self._sm._last_deep_states_list = None + else: Logger.localerr('Mirror synchronize request id=' f'{msg.behavior_id} mismatch active= {self._active_id}') @@ -511,7 +512,8 @@ def _onboard_heartbeat_callback(self, msg): f' Check UI and consider manual re-sync!\n' ' (mismatch may be temporarily understandable for rapidly changing outcomes)' f' {self._sync_heartbeat_mismatch_counter}') - Logger.localinfo(f'IDs {msg.behavior_id} {self._active_id} {self._sync_heartbeat_mismatch_counter}: \n' + Logger.localinfo(f'IDs {msg.behavior_id} {self._active_id}' + f' {self._sync_heartbeat_mismatch_counter}: \n' f' Onboard IDs: {msg.current_state_checksums}\n' f' Mirror IDs: {mirror_status.current_state_checksums}') @@ -519,16 +521,16 @@ def _onboard_heartbeat_callback(self, msg): try: ob_state_id, ob_out = StateMap.unhash(state_hash) ob_state = self._state_map[ob_state_id] - Logger.localinfo(f" onboard {ob_state_id} : '{ob_state.name.replace('_mirror', '')}'" - f" out={ob_out} - {ob_state.path.replace('_mirror', '')}") + Logger.localinfo(f" onboard {ob_state_id:10d} : '{ob_state.name.replace('_mirror', ''):30s}'" + f" out={ob_out:3d} - {ob_state.path.replace('_mirror', '')}") except Exception as exc: # pylint: disable=W0703 Logger.localinfo(f' error for onboard state hash {state_hash} - {type(exc)} - {exc}') for state_hash in mirror_status.current_state_checksums: try: mr_state_id, mr_out = StateMap.unhash(state_hash) mr_state = self._state_map[mr_state_id] - Logger.localinfo(f" mirror {mr_state_id} : '{mr_state.name.replace('_mirror', '')}'" - f" out={mr_out} - {mr_state.path.replace('_mirror', '')}") + Logger.localinfo(f" mirror {mr_state_id:10d} : '{mr_state.name.replace('_mirror', ''):30s}'" + f" out={mr_out:3d} - {mr_state.path.replace('_mirror', '')}") except Exception as exc: # pylint: disable=W0703 Logger.localinfo(f' error for mirror state hash {state_hash} - {type(exc)} - {exc}') Logger.localinfo(30 * '=') @@ -537,6 +539,20 @@ def _onboard_heartbeat_callback(self, msg): # Start counting mismatches self._sync_heartbeat_mismatch_counter = 1 else: + if self._sync_heartbeat_mismatch_counter > 0: + Logger.localwarn(f'OCS is back in sync after {self._sync_heartbeat_mismatch_counter} heartbeats') + Logger.localinfo(f'IDs {msg.behavior_id} {self._active_id}' + f' Onboard IDs: {msg.current_state_checksums}\n' + f' Mirror IDs: {mirror_status.current_state_checksums}') + for state_hash in msg.current_state_checksums: + try: + ob_state_id, ob_out = StateMap.unhash(state_hash) + ob_state = self._state_map[ob_state_id] + Logger.localinfo(f" onboard {ob_state_id:10d} : '{ob_state.name.replace('_mirror', ''):30s}'" + f" out={ob_out:3d} - {ob_state.path.replace('_mirror', '')}") + except Exception as exc: # pylint: disable=W0703 + Logger.localinfo(f' error for onboard state hash {state_hash} - {type(exc)} - {exc}') + # Reset mismatch counter self._sync_heartbeat_mismatch_counter = 0 elif self._active_id != 0: @@ -648,7 +664,7 @@ def _restart_mirror(self, msg, restart_time): self._outcome_sub.remove_last_msg(Topics._OUTCOME_TOPIC, clear_buffer=True) MirrorState._last_state_id = None MirrorState._last_state_outcome = None - + MirrorState._last_target_id = None # reset any time that we build a new state machine try: self._starting_path = None if self._sm is not None and self._sm.id == msg.behavior_id: @@ -716,6 +732,13 @@ def _restart_mirror(self, msg, restart_time): Logger.localwarn(f" Restart SM with current top-level state = {curst.name if curst is not None else 'None'} " f'starting path={self._starting_path}') Logger.localinfo(f' active states = {self._sm.get_latest_status()}') + if self._sm._last_deep_states_list is not None and len(self._sm._last_deep_states_list) > 0: + # Make sure we update the UI with latest state + MirrorState.publish_update(self._sm._last_deep_states_list[-1].state_id) + for st in self._sm._last_deep_states_list: + Logger.localinfo(f" '{st.name:30s}' - '{st.path}' ") + else: + MirrorState.publish_update(curst.state_id) # Make sure we update the UI with latest state self._running = True # set running while we have sync lock self._starting = False self._active_id = msg.behavior_id @@ -753,9 +776,9 @@ def _execute_mirror(self, start_time): Logger.loginfo('Starting mirror in state ' + self._starting_path) self._starting_path = None - result = PreemptableStateMachine._preempted_name + result = State._preempted_name try: - result = self._sm.spin(start_time) + result = self._sm.spin(start_time, self._state_map) Logger.localinfo(f"Mirror finished spin with result '{result}' after {self.get_elapsed_str(start_time)} s") self._sm.destroy() except Exception as exc: @@ -767,7 +790,7 @@ def _execute_mirror(self, start_time): # so just try a simple print print('\n(_execute_mirror Traceback): Caught exception on preempt:\n%s' % str(exc), flush=True) print(traceback.format_exc().replace('%', '%%'), flush=True) - result = PreemptableStateMachine._preempted_name + result = State._preempted_name self._active_thread_start = None self._running = False @@ -788,8 +811,9 @@ def _mirror_state_machine(self, msg): self._add_node(msg, root) if self._sm: - Logger.localinfo(f"Constructed mirror for behavior id ='{self._sm.id}' - begin validation ...") + Logger.localinfo('---------------------------------') self._sm.id = msg.behavior_id + Logger.localinfo(f"Constructed mirror for behavior id ='{self._sm.id}' - begin validation ...") # verify checksums of all states for con_msg in msg.containers: if con_msg.path.find('/') != -1: @@ -801,6 +825,16 @@ def _mirror_state_machine(self, msg): f'({con_msg.state_id}) {con_msg.path}') else: raise KeyError(f'State id {con_msg.state_id} not found in {self._state_map}!') + + try: + state_ids, state_paths = list(zip(*self._state_map.items)) + state_map_msg = StateMapMsg(behavior_id=self._sm.id, + state_ids=state_ids, + state_paths=[path.replace('_mirror', '') for path in state_paths]) + self._state_map_pub.publish(state_map_msg) # Used by the WebUI + except Exception as exc: + Logger.localerr(f'Failed to publish state map: {exc}') + end = time.time() Logger.localinfo(f"Validated constructed mirror for behavior id ='{self._sm.id}' in {end - start} seconds !") return # success here @@ -809,9 +843,6 @@ def _mirror_state_machine(self, msg): except Exception as exc: Logger.localwarn(f"_mirror_statemachine Exception: '{type(exc)}' - {exc}") - # self.get_logger().warn(f"{traceback.format_exc().replace('%', '%%')}") - # print(f"\x1b[93m{self._state_map}", flush=True) - # print('------------------\x1b[0m', flush=True) if self._sm is not None: Logger.localwarn(f' destroy constructed SM id={self._sm.id} - failed validation!') self._sm.destroy() @@ -846,7 +877,7 @@ def _add_node(self, msg, path): else: sm = MirrorStateMachine(container_name, path, outcomes=sm_outcomes) - self._state_map.add_state(path, sm) + self._state_map.add_state(path, sm) # also calculates the state id given path assert sm.state_id == container.state_id, ('Failed to validate container state_id ' f'= {sm.state_id} vs. {container.state_id}') @@ -860,9 +891,7 @@ def _add_node(self, msg, path): MirrorStateMachine.add(container_name + '_mirror', sm, transitions=container_transitions) else: # Add instance attributes to top-level state machine - sm._state_map = self._state_map sm._total_loop_count = 0 - self._sm = sm else: diff --git a/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py b/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py index 8d3b580..0108e69 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py +++ b/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py @@ -30,7 +30,8 @@ """Simplified state machine for use with FlexBE UI State machine mirror.""" from flexbe_core import Logger -from flexbe_core.core import StateMachine +from flexbe_core.core import State, StateMachine +from flexbe_core.core import StateMap from flexbe_mirror.mirror_state import MirrorState from flexbe_mirror.mirror_state_machine import MirrorStateMachine @@ -56,7 +57,7 @@ def on_enter_mirror(self, userdata): state._entering = True # force state to handle enter on first execute state._last_execution = None - MirrorState.publish_update(self._target_path) + MirrorState.publish_update(self.state_id) def on_exit_mirror(self, userdata, desired_outcome=-1, states=None): """Exit state and prepare for next entry (outcome -1 means preempt).""" @@ -69,7 +70,12 @@ def on_exit_mirror(self, userdata, desired_outcome=-1, states=None): self._current_state = None self._returned_outcomes = {} - self._last_outcome = self.outcomes[desired_outcome] + if desired_outcome != -1: + if desired_outcome == StateMap._MAX_OUTCOME: + self._last_outcome = State._preempted_name + else: + self._last_outcome = self.outcomes[desired_outcome] + MirrorState.publish_update(self.state_id + 255) # publish that we "entered" container to exit return self._last_outcome def execute_mirror(self, userdata): @@ -84,7 +90,8 @@ def execute_mirror(self, userdata): if MirrorState._last_state_id == self.state_id: # Handle outcome of this internal SM if self._last_outcome is not None: - Logger.localwarn(f"Mirror SM concurrency execute for '{self.name}' ({self._state_id}) : " + Logger.localwarn(f"Mirror SM concurrency execute for '{self.name.replace('_mirror', '')}' of " + f" '{self.path.replace('_mirror', '')}' ({self.state_id}) : " f"Already processed outcome='{self._last_outcome}' for " f'outcome index={MirrorState._last_state_outcome} - reprocessing anyway') @@ -113,6 +120,10 @@ def _execute_current_state_mirror(self, userdata): else: self._current_state.append(state) # Handle container return in execute + if len(self._current_state) == 0: + # No unexited states in concurrent, so notify we have returned to concurrent level + # Logger.localinfo(f"Inside CC '{self}' - no active internal - publish update '{self._target_path}'") + MirrorState.publish_update(self.state_id) # Notify back at top-level before exit return None def get_deep_states(self): @@ -133,9 +144,9 @@ def get_deep_states(self): else: deep_states.append(state) elif self._current_state is not None: - Logger.localerr(f"MirrorConcurrentContainer.get_deep_states '{self.name}' ({self._current_state._state_id})\n" + Logger.localerr(f"MirrorConcurrentContainer.get_deep_states '{self.name}' ({self._current_state.state_id})\n" f" - current state is NOT a list! Error type='{type(self._current_state)}'") - Logger.localerr(f" '{self._current_state.name}' ({self._current_state._state_id})") + Logger.localerr(f" '{self._current_state.name}' ({self._current_state.state_id})") raise TypeError(f"MirrorConcurrentContainer.get_deep_states '{self.name}' - " - f"current state is NOT a list! Errror type='{type(self._current_state)}'") + f"current state is NOT a list! Error type='{type(self._current_state)}'") return deep_states diff --git a/flexbe_mirror/flexbe_mirror/mirror_state.py b/flexbe_mirror/flexbe_mirror/mirror_state.py index 3484daf..0d29314 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_state.py +++ b/flexbe_mirror/flexbe_mirror/mirror_state.py @@ -31,11 +31,12 @@ from flexbe_core import EventState from flexbe_core import Logger -from flexbe_core.core.preemptable_state import PreemptableState -from flexbe_core.core.topics import Topics +from flexbe_core.core import State +from flexbe_core.core import StateMap +from flexbe_core.core import Topics from flexbe_core.proxy import ProxyPublisher -from std_msgs.msg import String +from std_msgs.msg import Int32 class MirrorState(EventState): @@ -49,7 +50,7 @@ class MirrorState(EventState): # Hold data from last outcome message being processed by mirror _last_state_id = None _last_state_outcome = None - _last_target_path = None + _last_target_id = None _pub = None @@ -57,21 +58,21 @@ def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy): """Initialize MirrorState instance.""" # pylint: disable=unused-argument super().__init__(outcomes=given_outcomes) - self._outcomes.append(PreemptableState._preempted_name) # Add preempted to outcomes list (for -1 outcome) + # self._outcomes.append(State._preempted_name) # Add preempted to outcomes list (for -1 outcome) self._target_name = target_name self._target_path = '/' + '/'.join(target_path.split('/')[1:]) # Drop top-level name - MirrorState._last_target_path = None # reset any time that we build a new state machine + MirrorState._last_target_id = None # reset any time that we build a new state machine if MirrorState._pub is None: - # Allow access to standard proxies initialied by flexbe_mirror + # Allow access to standard proxies initialized by flexbe_mirror MirrorState._pub = ProxyPublisher() @classmethod - def publish_update(cls, target_path): + def publish_update(cls, target_id): """Publish latest deep state for UI control.""" - if target_path != MirrorState._last_target_path: - MirrorState._last_target_path = target_path - MirrorState._pub.publish(Topics._BEHAVIOR_UPDATE_TOPIC, String(data=target_path)) + if target_id != MirrorState._last_target_id: + MirrorState._last_target_id = target_id + MirrorState._pub.publish(Topics._BEHAVIOR_UPDATE_TOPIC, Int32(data=target_id)) def execute_mirror(self, userdata): """Execute the mirror state.""" @@ -97,12 +98,16 @@ def on_enter_mirror(self, userdata): self._entering = False self._last_outcome = None self._last_execution = None - MirrorState.publish_update(self._target_path) + MirrorState.publish_update(self.state_id) def on_exit_mirror(self, userdata, desired_outcome): """Exit mirror state.""" try: - self._last_outcome = self.outcomes[desired_outcome] + if desired_outcome != -1: + if desired_outcome == StateMap._MAX_OUTCOME: + self._last_outcome = State._preempted_name + else: + self._last_outcome = self.outcomes[desired_outcome] return self._last_outcome except Exception as exc: # pylint: disable=W0703 Logger.localerr(f"Error: MirrorState execute for '{self.name}': " diff --git a/flexbe_mirror/flexbe_mirror/mirror_state_machine.py b/flexbe_mirror/flexbe_mirror/mirror_state_machine.py index 6477a30..23efa41 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_state_machine.py +++ b/flexbe_mirror/flexbe_mirror/mirror_state_machine.py @@ -33,10 +33,11 @@ from flexbe_core import Logger from flexbe_core.core import PreemptableState, PreemptableStateMachine +from flexbe_core.core import State +from flexbe_core.core import StateError from flexbe_core.core import StateMachine from flexbe_core.core import StateMap -from flexbe_core.core.exceptions import StateError -from flexbe_core.core.topics import Topics +from flexbe_core.core import Topics from flexbe_core.proxy import ProxySubscriberCached from flexbe_mirror.mirror_state import MirrorState @@ -59,11 +60,12 @@ def __init__(self, target_name, target_path, *args, **kwargs): self._target_name = target_name self._target_path = '/' + '/'.join(target_path.split('/')[1:]) # Drop top-level name - def spin(self, start_time, userdata=None): + def spin(self, start_time, state_map): """Spin the execute in loop for Mirror.""" Logger.localinfo(f"Mirror: begin spinning for '{self.name}' ({self.id}) " f' in thread with start time = {start_time.nanoseconds} ns') + userdata = None # Not used in mirror timing_event = Event() # Only the top-level SM needs the outcome topic subscribed by mirror @@ -73,13 +75,14 @@ def spin(self, start_time, userdata=None): MirrorState._last_state_outcome = None MirrorStateMachine._execute_flag = True # Force a first pass regardless of messages + self._last_deep_states_list = None # Force change to send behavior update loop_count = 0 self._total_loop_count = 0 # Attribute only added to top-level SM - outcome = PreemptableState._preempted_name + outcome = State._preempted_name if self._current_state is None: self.on_enter_mirror(userdata) Logger.localinfo(f"Mirror: set initial state for top-level '{self.name}' ({self.state_id}) ({self.id}) " - f" with state = {self._current_state.name}' ({self._current_state._state_id})") + f" with state = {self._current_state.name}' ({self._current_state.state_id})") while rclpy.ok() and not PreemptableState.preempt: self._total_loop_count += 1 @@ -91,7 +94,8 @@ def spin(self, start_time, userdata=None): # We will process every message to ensure consistency msg = outcome_sub.get_from_buffer(Topics._OUTCOME_TOPIC) state_id, outcome = StateMap.unhash(msg.data) - # Logger.localinfo(f" outcome {state_id} {outcome} in thread started at {start_time.nanoseconds}") + # Logger.localinfo(f" received outcome '{outcome}' " + # f"for '{state_map[state_id].path.replace('_mirror', '')}' ({state_id})") # Store data for handling by execute function in appropriate state MirrorState._last_state_id = state_id @@ -99,19 +103,20 @@ def spin(self, start_time, userdata=None): if MirrorState._last_state_id == self.state_id: # Handle this top-level outcome if self._last_outcome is not None: - Logger.localwarn(f"Mirror SM top-level spin for '{self.name}' : " + Logger.localwarn(f"Mirror SM top-level spin for '{self.name.replace('_mirror', '')}' " + f"of '{self.path.replace('_mirror', '')}: " f"Already processed outcome='{self._last_outcome}' for " - f" state '{self.name}' ({self.state_id}) given new " + f" state '{self.name.replace('_mirror', '')}' ({self.state_id}) given new " f'outcome index={MirrorState._last_state_outcome} - ' f'reprocessing anyway in thread started at {start_time.nanoseconds}') MirrorState._last_state_id = None # Flag that the message was handled if MirrorState._last_state_outcome is not None: outcome = self.on_exit_mirror(userdata, MirrorState._last_state_outcome) - MirrorState.publish_update(self._target_path) # Notify back at top-level before exit + MirrorState.publish_update(self.state_id) # Notify back at top-level before exit MirrorState._last_state_outcome = None # Flag that the message was handled - Logger.localinfo(f' top-level outcome {outcome} for {state_id} ' - f'in thread started at {start_time.nanoseconds}') + # Logger.localinfo(f' top-level outcome {outcome} for {state_id} ' + # f'in thread started at {start_time.nanoseconds}') break # Outcome at the top-level # Some change to process @@ -125,7 +130,7 @@ def spin(self, start_time, userdata=None): if MirrorState._last_state_id is not None or MirrorState._last_state_outcome is not None: # This should not happen unless system is out of sync - Logger.logwarn(f"MirrorStateMachine '{self.name}' ({self._state_id}) spin() - " + Logger.logwarn(f"MirrorStateMachine '{self.name}' ({self.state_id}) spin() - " f'no state handled outcome from {MirrorState._last_state_id} ' f'outcome index={MirrorState._last_state_outcome}') @@ -136,21 +141,15 @@ def spin(self, start_time, userdata=None): with self._status_lock: self._last_deep_states_list = deep_states - # In case of internal return in concurrency container send another update to UI - # for the deepest active state - if len(deep_states) > 0: - MirrorState.publish_update(deep_states[-1]._target_path) - if outcome is not None: - Logger.localinfo(f"MirrorStateMachine '{self.name}' ({self._state_id}) spin() - outcome = {outcome}" + Logger.localinfo(f"MirrorStateMachine '{self.name}' ({self.state_id}) spin() - outcome = {outcome}" ' - wait for confirming top-level outcome message!') else: # Process fast independent of simulation time in order to keep up with onboard - if loop_count > 50000: + if loop_count > 100000: loop_count = 0 # periodic spam for updates - Logger.localinfo(f" SM spinner -'{self.name}' ({self.id}) - " - f'after {self._total_loop_count} spins in thread started at {start_time.nanoseconds}') + Logger.localinfo(f" SM spinner -'{self.name}' ({self.id}) - {self._total_loop_count} spins") timing_event.wait(0.0002) # minor wait for next message if we didn't process anything previous loop except Exception as exc: # pylint: disable=W0703 @@ -192,16 +191,20 @@ def _execute_current_state_mirror(self, userdata): target = self._transitions[self._current_state.name][outcome] self._current_state = self._labels.get(target) # Get the new state if self._current_state is None: - return target + # Logger.localinfo(f"SM {self.name.replace('_mirror', '')} is done, but wait for outcome message.") + MirrorState.publish_update(self.state_id) # Notify back at sm-level before exit + return None else: + # Logger.localinfo(f"SM {self.name.replace('_mirror', '')} transitioning " + # f"to '{self._current_state.name.replace('_mirror', '')}' ...") self._current_state._entering = True return None except KeyError as exc: err_msg = f"Returned outcome '{outcome}' is not registered as a transition from '{self._current_state}'" - Logger.localerr(f"Mirror SM execute for '{self.name}' ({self._state_id}): {err_msg}") - Logger.localinfo(f' {self.name} ({self._state_id}) - labels={self._labels}') - Logger.localinfo(f' {self.name} ({self._state_id}) - transitions={self._transitions}') - Logger.localinfo(f' {self.name} ({self._state_id}) - outcomes={self._outcomes}') + Logger.localerr(f"Mirror SM execute for '{self.name}' ({self.state_id}): {err_msg}") + Logger.localinfo(f' {self.name} ({self.state_id}) - labels={self._labels}') + Logger.localinfo(f' {self.name} ({self.state_id}) - transitions={self._transitions}') + Logger.localinfo(f' {self.name} ({self.state_id}) - outcomes={self._outcomes}') raise StateError(err_msg) from exc # we handle internal SM transitions using mirror outcome messages @@ -214,8 +217,10 @@ def execute_mirror(self, userdata): if MirrorState._last_state_id == self.state_id: # Handle outcome of this internal SM + # Logger.localinfo(f"Handling outcome of SM '{self.name.replace('_mirror', '')}' " + # f"of '{self.path.replace('_mirror', '')}' ...") if self._last_outcome is not None: - Logger.localwarn(f"Mirror SM execute for '{self.name}' ({self._state_id}) : " + Logger.localwarn(f"Mirror SM execute for '{self.name}' ({self.state_id}) : " f'Already processed outcome={self._last_outcome} for ' f'outcome index={MirrorState._last_state_outcome} - reprocessing anyway') @@ -237,8 +242,8 @@ def get_deep_states(self): @return: The list of active states (not state machine) """ if isinstance(self._current_state, StateMachine): - return self._current_state.get_deep_states() - return [self._current_state] if self._current_state is not None else [] # Return as a list + return [self] + self._current_state.get_deep_states() + return [self, self._current_state] if self._current_state is not None else [self] # Return as a list def get_latest_status(self): """Return the latest execution information as a BehaviorSync message.""" @@ -271,17 +276,22 @@ def on_enter_mirror(self, userdata): self._last_outcome = None self._current_state._entering = True # force state to handle enter on first execute self._userdata = None # not used in mirror - MirrorState.publish_update(self._target_path) + MirrorState.publish_update(self.state_id) def on_exit_mirror(self, userdata, desired_outcome=-1): """Exit mirror statemachine.""" try: if self._current_state is not None: self._current_state._entering = True - self._current_state.on_exit(userdata, -1) # Preempted - self._last_outcome = self.outcomes[desired_outcome] + self._current_state.on_exit_mirror(userdata, -1) # Preempted + if desired_outcome != -1: + if desired_outcome == StateMap._MAX_OUTCOME: + self._last_outcome = State._preempted_name + else: + self._last_outcome = self.outcomes[desired_outcome] self._current_state = None self._entering = True + MirrorState.publish_update(self.state_id + 255) # publish that we "entered" container to exit return self._last_outcome except Exception: # pylint: disable=W0703 Logger.localerr(f"Error: MirrorStateMachine execute for '{self.name}': " diff --git a/flexbe_mirror/package.xml b/flexbe_mirror/package.xml index e6ddc26..8c1b019 100644 --- a/flexbe_mirror/package.xml +++ b/flexbe_mirror/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> flexbe_mirror - 3.0.6 + 4.0.1 flexbe_mirror implements functionality to remotely mirror an executed behavior. diff --git a/flexbe_mirror/setup.py b/flexbe_mirror/setup.py index e2473e7..2c84118 100644 --- a/flexbe_mirror/setup.py +++ b/flexbe_mirror/setup.py @@ -7,7 +7,7 @@ setup( name=PACKAGE_NAME, - version='3.0.6', + version='4.0.1', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]), diff --git a/flexbe_msgs/CHANGELOG.rst b/flexbe_msgs/CHANGELOG.rst index 30f99eb..545d6fa 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.0.1 (2024-09-26) +------------------ +* codespell clean up + +4.0.0 (2024-08-24) +------------------ +* Version 4.0.0 release using state_id for communication +* this breaks API with flexbe_app and requires version 4.1.0+ of the FlexBE WebUI API +* use state id consistently to avoid long path strings +* modify preempt and published outcome to improve sync +* add flexbe_outcome_listener node for simple monitoring + +3.0.7 (2024-08-24) +------------------ +* add new states; modify BehaviorInput to allow strings and selection combo box + 3.0.6 (2024-08-05) ------------------ diff --git a/flexbe_msgs/CMakeLists.txt b/flexbe_msgs/CMakeLists.txt index 767742a..67bb3db 100644 --- a/flexbe_msgs/CMakeLists.txt +++ b/flexbe_msgs/CMakeLists.txt @@ -17,6 +17,7 @@ set(msg_files "msg/OutcomeCondition.msg" "msg/OutcomeRequest.msg" "msg/StateInstantiation.msg" + "msg/StateMapMsg.msg" "msg/SynthesisErrorCodes.msg" "msg/SynthesisRequest.msg" "msg/UICommand.msg" diff --git a/flexbe_msgs/action/BehaviorExecution.action b/flexbe_msgs/action/BehaviorExecution.action index fe60536..94b92aa 100644 --- a/flexbe_msgs/action/BehaviorExecution.action +++ b/flexbe_msgs/action/BehaviorExecution.action @@ -18,5 +18,5 @@ string outcome --- -# path of the current state -string current_state +# state id of the current state +int32 current_state diff --git a/flexbe_msgs/action/BehaviorInput.action b/flexbe_msgs/action/BehaviorInput.action index 8231282..2af2113 100644 --- a/flexbe_msgs/action/BehaviorInput.action +++ b/flexbe_msgs/action/BehaviorInput.action @@ -1,4 +1,4 @@ -# If desired, custom request types can be defined and choosen here +# If desired, custom request types can be defined and chosen here uint8 request_type # Basic types (used by input_action_server, but otherwise optional) @@ -9,12 +9,16 @@ uint8 REQUEST_3D = 3 # List of 3 floats uint8 REQUEST_4D = 4 # List of 4 floats (e.g. quaternion) uint8 REQUEST_POSE = 10 # Geometry message pose uint8 REQUEST_POSE_STAMPED = 11 # Geometry message pose stamped - +uint8 REQUEST_STRING = 12 # String +uint8 REQUEST_SELECTION = 13 # Select from among items in list # Request message displayed to the operator # Provide context information, i.e. for which purpose the data is required. string msg +# List of items for selection (used only by REQUEST_SELECTION ) +string[] items + --- uint8 RESULT_OK = 0 diff --git a/flexbe_msgs/action/BehaviorSynthesis.action b/flexbe_msgs/action/BehaviorSynthesis.action index 6fced67..e6ede7d 100644 --- a/flexbe_msgs/action/BehaviorSynthesis.action +++ b/flexbe_msgs/action/BehaviorSynthesis.action @@ -6,7 +6,7 @@ SynthesisRequest request --- -# Provides feedback regarding possibly occured errors +# Provides feedback regarding possibly occurred errors SynthesisErrorCodes error_code # Result of behavior synthesis as a list of state instantiations diff --git a/flexbe_msgs/msg/Container.msg b/flexbe_msgs/msg/Container.msg index 83bcb34..6182694 100644 --- a/flexbe_msgs/msg/Container.msg +++ b/flexbe_msgs/msg/Container.msg @@ -1,4 +1,4 @@ -int32 state_id # Unique 23-bit identifer for state +int32 state_id # Unique 23-bit identifier for state string path # String path from top-level state machine string[] children string[] outcomes diff --git a/flexbe_msgs/msg/OutcomeRequest.msg b/flexbe_msgs/msg/OutcomeRequest.msg index 620e985..8842292 100644 --- a/flexbe_msgs/msg/OutcomeRequest.msg +++ b/flexbe_msgs/msg/OutcomeRequest.msg @@ -1,2 +1,2 @@ uint8 outcome -string target \ No newline at end of file +int32 target # state id \ No newline at end of file diff --git a/flexbe_msgs/msg/StateMapMsg.msg b/flexbe_msgs/msg/StateMapMsg.msg new file mode 100644 index 0000000..ebe2036 --- /dev/null +++ b/flexbe_msgs/msg/StateMapMsg.msg @@ -0,0 +1,8 @@ +# Mapping between numeric ids and state path in given behavior + +int32 behavior_id # Behavior ID assigned in ContainerStructure.msg + +# Ordered lists of corresponding state id hash code and state path +int32[] state_ids +string[] state_paths + diff --git a/flexbe_msgs/package.xml b/flexbe_msgs/package.xml index 3d02a40..8d0f669 100644 --- a/flexbe_msgs/package.xml +++ b/flexbe_msgs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> flexbe_msgs - 3.0.6 + 4.0.1 flexbe_msgs provides the messages used by FlexBE. diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index dc60854..6c095dc 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.0.1 (2024-09-26) +------------------ +* codespell clean up +* rename to flexbe_status_listener with heartbeat and sync updates +* use latched state_map topic +* remove some stray spam +* use jazzy test and increase some loop timings in tests due to intermittent test failure + +4.0.0 (2024-08-24) +------------------ +* Version 4.0.0 release using state_id for communication +* this breaks API with flexbe_app and requires version 4.1.0+ of the FlexBE WebUI API +* use state id consistently to avoid long path strings +* modify preempt and published outcome to improve sync +* add flexbe_outcome_listener node for simple monitoring + +3.0.7 (2024-08-24) +------------------ +* add initialize_flexbe_core for common initialization + 3.0.6 (2024-08-05) ------------------ * minor clean up from flake8/pycodestyle diff --git a/flexbe_onboard/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py index fac98c1..f89c41e 100644 --- a/flexbe_onboard/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py @@ -55,7 +55,7 @@ def set_thread_name(name): from flexbe_core.core.topics import Topics from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached -from flexbe_msgs.msg import BEStatus, BehaviorSelection, BehaviorSync, CommandFeedback, UserdataInfo +from flexbe_msgs.msg import BEStatus, BehaviorSelection, BehaviorSync, CommandFeedback, StateMapMsg, UserdataInfo from flexbe_msgs.srv import GetUserdata import rclpy @@ -94,7 +94,10 @@ def __init__(self): self._heartbeat_pub = self.create_publisher(BehaviorSync, Topics._ONBOARD_HEARTBEAT_TOPIC, 10) self._status_pub = self.create_publisher(BEStatus, Topics._ONBOARD_STATUS_TOPIC, 10) + # Latch state map so we can retrieve later if desired latching_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + self._state_map_pub = self.create_publisher(StateMapMsg, Topics._STATE_MAP_TOPIC, qos_profile=latching_qos) + self._version_sub = self.create_subscription(String, Topics._UI_VERSION_TOPIC, self._version_callback, qos_profile=latching_qos) @@ -158,7 +161,7 @@ def _behavior_callback(self, beh_sel_msg): Logger.logwarn(f'Received behavior start request for {beh_sel_msg.behavior_key} ' f'({beh_sel_msg.behavior_id}) while prior request was starting.\n Ignore second request!') return - self._starting = True # Prevent two start requests from ocurring back to back + self._starting = True # Prevent two start requests from occurring back to back self._trigger_ready = False # We have received the behavior selection request self._ready_counter = 0 thread = threading.Thread(target=self._behavior_execution, args=[beh_sel_msg]) @@ -315,7 +318,15 @@ def _behavior_execution(self, beh_sel_msg): self.be.confirm() Logger.localinfo(f' behavior {self.be.name}: {self.be.beh_id} confirmation.') - args = [self.be.requested_state_path] if self.be.requested_state_path is not None else [] + # Publish behavior state map as a debugging aid (match to OCS side published by launcher and mirror) + state_ids, state_paths = be.state_map_items + state_map_msg = StateMapMsg(behavior_id=be.beh_id, + state_ids=state_ids, + state_paths=state_paths) + self._state_map_pub.publish(state_map_msg) + + # Publish start status + args = [self.be.requested_state_id] if self.be.requested_state_id is not None else [] Logger.localinfo(f'Behavior Engine - behavior {self.be.name}: {self.be.beh_id} BEStatus STARTED.') self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), behavior_id=self.be.beh_id, diff --git a/flexbe_onboard/package.xml b/flexbe_onboard/package.xml index 8068892..b0b1c1b 100644 --- a/flexbe_onboard/package.xml +++ b/flexbe_onboard/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> flexbe_onboard - 3.0.6 + 4.0.1 flexbe_onboard implements the robot-side of the behavior engine from where all behaviors are started. diff --git a/flexbe_onboard/setup.py b/flexbe_onboard/setup.py index a25463c..e3a9b24 100644 --- a/flexbe_onboard/setup.py +++ b/flexbe_onboard/setup.py @@ -9,7 +9,7 @@ setup( name=package_name, - version='3.0.6', + version='4.0.1', packages=find_packages(), data_files=[ (os.path.join('share', package_name), glob('launch/*.launch.py')), diff --git a/flexbe_onboard/tests/flexbe_onboard_test_data/complex_behavior_test_sm.py b/flexbe_onboard/tests/flexbe_onboard_test_data/complex_behavior_test_sm.py index 51dffd9..89ba309 100644 --- a/flexbe_onboard/tests/flexbe_onboard_test_data/complex_behavior_test_sm.py +++ b/flexbe_onboard/tests/flexbe_onboard_test_data/complex_behavior_test_sm.py @@ -39,7 +39,7 @@ __test__ = False # Do not pytest this class (it is the test!) -from flexbe_INVALID import Autonomy, Behavior, OperatableStateMachine +from flexbe_INVALID import Autonomy, Behavior, OperatableStateMachine, initialize_flexbe_core from flexbe_states.calculation_state import CalculationState from flexbe_states.decision_state import DecisionState @@ -75,12 +75,7 @@ def __init__(self, node): self.name = 'Complex Behavior Test' self.node = node - WaitState.initialize_ros(node) - DecisionState.initialize_ros(node) - CalculationState.initialize_ros(node) - flexbe_states__LogState.initialize_ros(node) - - OperatableStateMachine.initialize_ros(node) + initialize_flexbe_core(node) # parameters of this behavior self.add_parameter('param', 'value_1') diff --git a/flexbe_onboard/tests/flexbe_onboard_test_data/log_behavior_test_sm.py b/flexbe_onboard/tests/flexbe_onboard_test_data/log_behavior_test_sm.py index 062ec19..1ae7833 100644 --- a/flexbe_onboard/tests/flexbe_onboard_test_data/log_behavior_test_sm.py +++ b/flexbe_onboard/tests/flexbe_onboard_test_data/log_behavior_test_sm.py @@ -37,7 +37,7 @@ # Only code inside the [MANUAL] tags will be kept. # ########################################################### -from flexbe_core import Autonomy, Behavior, OperatableStateMachine +from flexbe_core import Autonomy, Behavior, OperatableStateMachine, initialize_flexbe_core from flexbe_states.log_state import LogState as flexbe_states__LogState from flexbe_states.wait_state import WaitState as flexbe_states__WaitState @@ -66,9 +66,7 @@ def __init__(self, node): super(LogBehaviorTestSM, self).__init__() self.name = 'Log Behavior Test' self.node = node - flexbe_states__WaitState.initialize_ros(node) - flexbe_states__LogState.initialize_ros(node) - OperatableStateMachine.initialize_ros(node) + initialize_flexbe_core(node) # parameters of this behavior diff --git a/flexbe_onboard/tests/test_onboard.py b/flexbe_onboard/tests/test_onboard.py index 6ae9743..0370419 100755 --- a/flexbe_onboard/tests/test_onboard.py +++ b/flexbe_onboard/tests/test_onboard.py @@ -211,7 +211,7 @@ def test_onboard_behaviors(self): self.clear_extra_heartbeat_ready_messages() # send the same behavior with different parameters - self.node.get_logger().info('Republish modified behavior ...') + self.node.get_logger().info('\n\nRepublish modified behavior ...') request.arg_keys = ['param', 'invalid'] request.arg_values = ['value_1', 'should be ignored'] request.input_keys = [] @@ -224,13 +224,21 @@ def test_onboard_behaviors(self): behavior_logs = [] # Wait for published message - end_time = time.time() + 1 - while time.time() < end_time: - self.executor.spin_once(timeout_sec=0.1) + self.node.get_logger().info('\n\nExecute modified behavior ...') + end_time = time.time() + 2 + try: + while time.time() < end_time: + self.executor.spin_once(timeout_sec=0.1) + except Exception as exc: + print(f'\x1b[91mException in executor: {exc}\x1b[0m') while self.sub.has_buffered(Topics._BEHAVIOR_LOGGING_TOPIC): behavior_logs.append(self.sub.get_from_buffer(Topics._BEHAVIOR_LOGGING_TOPIC).text) - self.executor.spin_once(timeout_sec=0.1) + try: + self.executor.spin_once(timeout_sec=0.1) + except Exception as exc: + print(f'\x1b[91mException in executor: {exc}\x1b[0m') + self.node.get_logger().info(f'{behavior_logs}') self.assertIn('value_1', behavior_logs) self.node.get_logger().info('Done onboard testing!') self.executor.spin_once(timeout_sec=0.1) diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index e94bf5c..510e410 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.0.1 (2024-09-26) +------------------ +* codespell clean up + +4.0.0 (2024-08-24) +------------------ +* Version 4.0.0 release using state_id for communication +* this breaks API with flexbe_app and requires version 4.1.0+ of the FlexBE WebUI API +* use state id consistently to avoid long path strings +* modify preempt and published outcome to improve sync +* add flexbe_outcome_listener node for simple monitoring + +3.0.7 (2024-08-24) +------------------ +* add new states; modify BehaviorInput to allow strings and selection combo box +* add initialize_flexbe_core for common initialization + 3.0.6 (2024-08-05) ------------------ * update cancel for action client; define get_status to replace get_state for proxy action client diff --git a/flexbe_states/flexbe_states/input_state.py b/flexbe_states/flexbe_states/input_state.py index 77b2ed9..6b13ac1 100644 --- a/flexbe_states/flexbe_states/input_state.py +++ b/flexbe_states/flexbe_states/input_state.py @@ -47,7 +47,7 @@ class InputState(EventState): Requests of different types, such as requesting a waypoint, a template, or a pose, can be specified. -- request uint8 One of the custom-defined values to specify the type of request. - -- message string Message displayed to the operator to let him know what to do. + -- message string Message displayed to the operators to let them know what to do. -- timeout float Timeout in seconds to wait for server to be available. #> data object The data provided by the operator. The exact type depends on the request. @@ -101,14 +101,19 @@ def execute(self, userdata): # Attempt to load data and convert it to the proper format. try: # Convert string to byte array and load using pickle - input_data = ast.literal_eval(result.data) + if self._request in (BehaviorInput.Goal.REQUEST_STRING, BehaviorInput.Goal.REQUEST_SELECTION): + Logger.localinfo(f" InputState returned string '{result.data}'") + response_data = result.data + else: + Logger.localinfo(f" InputState returned '{result.data}'") + input_data = ast.literal_eval(result.data) + + # Note: This state uses the Pickle module, and is subject to this warning from the Pickle manual: + # Warning: The pickle module is not secure against erroneous or maliciously constructed data. + # Never unpickle data received from an untrusted or unauthenticated source. + response_data = pickle.loads(input_data) + Logger.localinfo(f' converted to {type(response_data)} : {response_data}') - # Note: This state uses the Pickle module, and is subject to this warning from the Pickle manual: - # Warning: The pickle module is not secure against erroneous or maliciously constructed data. - # Never unpickle data received from an untrusted or unauthenticated source. - response_data = pickle.loads(input_data) - - Logger.localinfo(f' InputState returned {type(response_data)} : {response_data}') userdata.data = response_data self._return = 'received' except Exception as exc: # pylint: disable=W0703 @@ -131,9 +136,9 @@ def on_enter(self, userdata): self._client.remove_result(self._action_topic) self._return = None - # Retrive the goal for the BehaviorInput Action. + # Retrieve the goal for the BehaviorInput Action. action_goal = BehaviorInput.Goal() - # Retrive the request type and message from goal. + # Retrieve the request type and message from goal. action_goal.request_type = self._request action_goal.msg = self._message Logger.loghint(f"Onboard requests '{self._message}'") @@ -147,14 +152,16 @@ def on_enter(self, userdata): self._return = 'no_connection' def on_exit(self, userdata): + """Call when state exits.""" # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. Logger.localinfo(f"on exit for '{self._action_topic}'.") - if not self._client.has_result(self._action_topic): - Logger.localinfo(f"Request to cancel active action goal for '{self._action_topic}'.") + if self._client.is_active(self._action_topic): self._client.cancel(self._action_topic) - else: + Logger.loginfo(f"Cancelled active action goal for '{self._action_topic}'.") + + if self._client.has_result(self._action_topic): # remove the old result so we are ready for the next time # and don't prematurely return # Note: We don't delete in execute to allow the cancel check above diff --git a/flexbe_states/flexbe_states/log_key_state.py b/flexbe_states/flexbe_states/log_key_state.py index 106566d..8be6f87 100644 --- a/flexbe_states/flexbe_states/log_key_state.py +++ b/flexbe_states/flexbe_states/log_key_state.py @@ -36,9 +36,13 @@ class LogKeyState(EventState): """ A state that can log a predefined message including an input key. - Can be used to precisely inform the operator about what happened to the behavior. + The text should be a Python format string (e.g. 'Counter value: {}'), + where {} is a placeholder replaced by the value of the userdata.data using the + text.format(userdata.data) command. - -- text string The message to be logged to the terminal Example: 'Counter value: {}'. + This can be used to precisely inform the operator about what happened to the behavior. + + -- text string Format string of message to be logged to the terminal; e.g., 'Counter value: {}'. -- severity uint8 Type of logging (Logger.REPORT_INFO / WARN / HINT / ERROR) #> data object The data provided to be printed in the message. The exact type depends on the request. diff --git a/flexbe_states/flexbe_states/selection_state.py b/flexbe_states/flexbe_states/selection_state.py new file mode 100644 index 0000000..586ca76 --- /dev/null +++ b/flexbe_states/flexbe_states/selection_state.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""SelectionState.""" +from action_msgs.msg import GoalStatus + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyActionClient + +from flexbe_msgs.action import BehaviorInput + + +class SelectionState(EventState): + """ + Implements a state where the state machine needs an input from the operator as choice from list of strings. + + Requests of different types, such as requesting a waypoint, a template, or a pose, can be specified. + + -- message string Message displayed to the operators to let them know what to do. + -- timeout float Timeout in seconds to wait for server to be available. + + ># items object List or tuple of items to select from + #> data object The data selected by the operator. The exact type depends on the request. + + <= received Returned as soon as valid data is available. + <= aborted The operator declined to provide the requested data. + <= no_connection No request could be sent to the operator. + <= data_error Data has been received, but could not be deserialized successfully. + + Note: This state uses the Pickle module, and is subject to this warning from the Pickle manual: + Warning: The pickle module is not secure against erroneous or maliciously constructed data. + Never unpickle data received from an untrusted or unauthenticated source. + + If using this state to accept user input, it is up to the user to protect their network from untrusted data! + + """ + + def __init__(self, message, timeout=1.0, action_topic='flexbe/behavior_input'): + """Construct instance.""" + super(SelectionState, self).__init__(outcomes=['received', 'aborted', 'no_connection', 'data_error'], + input_keys=['items'], + output_keys=['data']) + ProxyActionClient.initialize(SelectionState._node) + self._action_topic = action_topic + self._client = None + self._message = message + self._timeout = timeout + self._return = None + + def on_start(self): + """Set up proxy action client for behavior input server on behavior start.""" + self._client = ProxyActionClient({self._action_topic: BehaviorInput}, wait_duration=0.0) + + def on_stop(self): + """Stop client when behavior stops.""" + ProxyActionClient.remove_client(self._action_topic) + self._client = None + + def execute(self, userdata): + """Execute state waiting for action result.""" + if self._return: + # Return prior value if blocked + return self._return + + if self._client.has_result(self._action_topic): + result = self._client.get_result(self._action_topic) + if result.result_code != BehaviorInput.Result.RESULT_OK: + userdata.data = None + self._return = 'aborted' + else: + # Attempt to load data and convert it to the proper format. + try: + # Selection state returns string + response_data = result.data + + Logger.loginfo(f" SelectionState returned '{response_data}'") + userdata.data = response_data + self._return = 'received' + except Exception as exc: # pylint: disable=W0703 + Logger.logwarn('Was unable to load provided data for ' + f"'{self._action_topic}':\n '{result.data}'\n " + f' {str(exc)}') + userdata.data = None + self._return = 'data_error' + elif self._client.get_status(self._action_topic) == GoalStatus.STATUS_CANCELED: + Logger.localinfo(f" InputState {self._action_topic}' goal was canceled! ") + self._return = 'aborted' + elif self._client.get_status(self._action_topic) == GoalStatus.STATUS_ABORTED: + Logger.localinfo(f" InputState {self._action_topic}' goal was aborted! ") + self._return = 'aborted' + + return self._return + + def on_enter(self, userdata): + """Send goal to action server on entering state.""" + self._client.remove_result(self._action_topic) + self._return = None + + if 'items' not in userdata: + self._return = 'aborted' + msg = f"SelectionState '{self}' requires userdata.items key!" + Logger.localwarn(msg) + return + + # Retrieve the goal for the BehaviorInput Action. + action_goal = BehaviorInput.Goal(request_type=BehaviorInput.Goal.REQUEST_SELECTION, + items=userdata.items, msg=self._message) + Logger.loghint(f"Onboard requests '{self._message}' : {userdata.items}") + + # Attempt to send the goal. + try: + self._client.send_goal(self._action_topic, action_goal, wait_duration=self._timeout) + # Logger.localinfo(f"Sent goal for '{self._action_topic}'.") + except Exception as exc: + Logger.logwarn(f"Was unable to send data request for '{self._action_topic}':\n {exc}") + self._return = 'no_connection' + + def on_exit(self, userdata): + """Call when state exits.""" + # Make sure that the action is not running when leaving this state. + # A situation where the action would still be active is for example when the operator manually triggers an outcome. + Logger.localinfo(f"on exit for '{self._action_topic}'.") + + if self._client.is_active(self._action_topic): + self._client.cancel(self._action_topic) + Logger.loginfo(f"Requested to cancel active action goal for '{self._action_topic}'.") diff --git a/flexbe_states/flexbe_states/user_data_state.py b/flexbe_states/flexbe_states/user_data_state.py new file mode 100644 index 0000000..3f37bfe --- /dev/null +++ b/flexbe_states/flexbe_states/user_data_state.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""UserdataState.""" +import copy + +from flexbe_core import EventState + + +class UserdataState(EventState): + """ + A state that posts data onto the userdata stream. + + -- data object The data to be posted + + <= done Indicates that data was posted + ># data object A copy of the data + """ + + def __init__(self, data): + super(UserdataState, self).__init__(outcomes=['done'], + output_keys=['data']) + self._data = data + + def execute(self, userdata): + """Execute UserdataState.""" + # Post data to userdata and return done. + userdata.data = copy.copy(self._data) + return 'done' diff --git a/flexbe_states/package.xml b/flexbe_states/package.xml index 09970b8..543cd11 100644 --- a/flexbe_states/package.xml +++ b/flexbe_states/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> flexbe_states - 3.0.6 + 4.0.1 flexbe_states provides a collection of common generic predefined states. diff --git a/flexbe_states/ros2-conversion-best-practices.md b/flexbe_states/ros2-conversion-best-practices.md index b6e83d6..762ffbe 100644 --- a/flexbe_states/ros2-conversion-best-practices.md +++ b/flexbe_states/ros2-conversion-best-practices.md @@ -6,10 +6,19 @@ the custom states behind the scenes. As a result, the biggest changes to custom FlexBE states focus on configuring the FlexBE proxies as well as handle any direct calls to ROS 1’s rospy package. -The following image shows the ROS 1 (left) and ROS 2 (right) versions of the `TimedTwistState` from [Flexible Navigation](https://github.com/flexbe/flexible_navigation.git), a FlexBE-based navigation system. +The following image shows the ROS 1 (left) and an early version of the ROS 2 (right) conversion of the +[`TimedTwistState`](https://github.com/FlexBE/flexible_navigation/blob/ros2-devel/flex_nav_flexbe_states/flex_nav_flexbe_states/timed_twist_state.py) +from [Flexible Navigation](https://github.com/flexbe/flexible_navigation.git), a FlexBE-based navigation system. ![ROS1 vs. ROS2 state implementations](./timed_twist_state_conversion.png) +> Note: the call to `ProxyPublisher.initialize(TimedTwistState._node)` is out of date in image; see[`TimedTwistState`](https://github.com/FlexBE/flexible_navigation/blob/ros2-devel/flex_nav_flexbe_states/flex_nav_flexbe_states/timed_twist_state.py) for the latest code. +As of version `3.1.0`, these calls within individual states can be avoided my making a single call to `initialize_flexbe_core` in +the behavior initialization; that is, `initialize` is not longer required at the state implementation level. +The `flexbe_webui` will now write behaviors with this call (the older `flexbe_app` does NOT call `initialize_flexbe_core` at this time). +Older behaviors should be updated to make this call, or be sure states are +properly initialized (e.g., `ProxyPublisher.initialize(TimedTwistState._node)`). For now, most existing states will retain these redundant calls to `initialize(self._node)` to maintain backward compatibility. New states do not require these if the proxies are initialized by the behavior using `initialize_flexbe_core`. + As can be seen in the ROS 2 version's constructor, the `ProxyPublisher` used to create a publisher to publish `TwistStamped` messages first needs to be initialized using the `TimedTwistState`'s node. This would also be done with a `ProxySubscriberCached`, `ProxyActionClient`, and `ProxyServiceCaller` to create a subscriber, action client, and service client respectively. After initializing, the creation of the proxy publisher, subscriber, action client, @@ -25,7 +34,7 @@ When setting the timestamp, the ROS 2 version is able to use `self._node` as opposed to `TimedTwistStamped._node` in the constructor. The reason is that during the creation of the custom state in the constructor the individual instance of the custom state does not have access to the node. -However, during execution the custom state instance has access to its node, +However, during execution the custom state instance has access to its node, where the `TimedTwistState` can use its node to get the current time. Like getting the time for the timestamp, each instance of directly using the `rospy` package needs to be changed to use the custom state’s node. diff --git a/flexbe_states/setup.py b/flexbe_states/setup.py index a7a927a..114bdc5 100644 --- a/flexbe_states/setup.py +++ b/flexbe_states/setup.py @@ -9,7 +9,7 @@ setup( name=PACKAGE_NAME, - version='3.0.6', + version='4.0.1', packages=find_packages(), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]), diff --git a/flexbe_testing/CHANGELOG.rst b/flexbe_testing/CHANGELOG.rst index 1d9be5a..0b405f3 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.0.1 (2024-09-26) +------------------ + +4.0.0 (2024-08-24) +------------------ +* Version 4.0.0 release using state_id for communication +* this breaks API with flexbe_app and requires version 4.1.0+ of the FlexBE WebUI API +* use state id consistently to avoid long path strings +* modify preempt and published outcome to improve sync +* add flexbe_outcome_listener node for simple monitoring + +3.0.7 (2024-08-24) +------------------ +* add initialize_flexbe_core for common initialization + 3.0.6 (2024-08-05) ------------------ diff --git a/flexbe_testing/flexbe_testing/test/selftest_behavior_sm.py b/flexbe_testing/flexbe_testing/test/selftest_behavior_sm.py index 3e14bbb..ddbc10f 100644 --- a/flexbe_testing/flexbe_testing/test/selftest_behavior_sm.py +++ b/flexbe_testing/flexbe_testing/test/selftest_behavior_sm.py @@ -32,7 +32,7 @@ """Content is only intended for the flexbe_testing self-test.""" -from flexbe_core import Autonomy, Behavior, EventState, Logger, OperatableStateMachine +from flexbe_core import Autonomy, Behavior, EventState, Logger, OperatableStateMachine, initialize_flexbe_core class SelftestBehaviorSM(Behavior): @@ -44,9 +44,7 @@ def __init__(self, node): self.name = 'Selftest Behavior' self.node = node - OperatableStateMachine.initialize_ros(self.node) - SelftestBehaviorSM._CalculationState.initialize_ros(self.node) - SelftestBehaviorSM._DecisionState.initialize_ros(self.node) + initialize_flexbe_core(self.node) # parameters of this behavior self.value = None # avoid pylint error diff --git a/flexbe_testing/package.xml b/flexbe_testing/package.xml index 918c2bc..9bf9768 100644 --- a/flexbe_testing/package.xml +++ b/flexbe_testing/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> flexbe_testing - 3.0.6 + 4.0.1 flexbe_testing provides a framework for unit testing states. diff --git a/flexbe_testing/setup.py b/flexbe_testing/setup.py index 9d1bdef..7336233 100644 --- a/flexbe_testing/setup.py +++ b/flexbe_testing/setup.py @@ -8,7 +8,7 @@ setup( name=package_name, - version='2.0.0', + version='4.0.1', packages=[package_name], data_files=[ (os.path.join('share', package_name), glob('launch/*.launch.py')), diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index c4c9d2d..a9fc6bc 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.0.1 (2024-09-26) +------------------ +* codespell clean up +* rename to flexbe_status_listener with heartbeat and sync updates +* use latched state_map topic + +4.0.0 (2024-08-24) +------------------ +* Version 4.0.0 release using state_id for communication +* this breaks API with flexbe_app and requires version 4.1.0+ of the FlexBE WebUI API +* use state id consistently to avoid long path strings +* modify preempt and published outcome to improve sync +* add flexbe_outcome_listener node for simple monitoring + +3.0.7 (2024-08-24) +------------------ +* update create_repo script + 3.0.6 (2024-08-05) ------------------ diff --git a/flexbe_widget/CMakeLists.txt b/flexbe_widget/CMakeLists.txt index 5abb5d1..1dcc9dc 100644 --- a/flexbe_widget/CMakeLists.txt +++ b/flexbe_widget/CMakeLists.txt @@ -11,6 +11,7 @@ install(PROGRAMS bin/breakpoint bin/create_repo bin/evaluate_logs + bin/flexbe_status_listener DESTINATION lib/${PROJECT_NAME} ) diff --git a/flexbe_widget/bin/create_repo b/flexbe_widget/bin/create_repo index ed6d090..4ac04ff 100755 --- a/flexbe_widget/bin/create_repo +++ b/flexbe_widget/bin/create_repo @@ -1,36 +1,48 @@ #!/bin/bash if [ $# -lt 1 ]; then echo -e "\e[93mPlease provide your desired project name as argument.\033[0m" + echo -e "\e[93m Usage: ros2 run flexbe_widget create_repo [meta_package_name] [--non-interactive] + echo -e "" + echo -e " By default the script will use the as the [meta_package_name] exit 2 fi -if [ $# -ne 2 ]; then - interactive=true -else - if [ $2 = "--non-interactive" ]; then - echo "Running in non-interactive mode for CI tests ..." - interactive=false - else - interactive=true - fi +name=$1 +meta_package_name=$1 +interactive=true + +echo -e "\e[96mInitializing new flexbe project repo \e[1m${name} ...\033[0m" + +# Clear interactive flag if either argument is --non-interactive +if [ "${2:-}" = "--non-interactive" ] || [ "${3:-}" = "--non-interactive" ]; then + echo "Running in non-interactive mode for CI tests ..." + interactive=false fi +# Set meta_package_name if $2 or $3 is provided and not "--non-interactive" +if [ -n "${2:-}" ] && [ "$2" != "--non-interactive" ]; then + meta_package_name="$2" +elif [ -n "${3:-}" ] && [ "$3" != "--non-interactive" ]; then + meta_package_name="$3" +fi + + # Check the desired directory -# Note, someone may want to put this in another folder, so it +# Note, someone may want to put this in another folder, so it # is not necessarily incorrect to run from different folder if [[ ":$WORKSPACE_ROOT/src:" != ":$PWD:" ]]; then echo -e "\e[93mNormally we run this command from the \$WORKSPACE_ROOT/src folder.\e[0m" echo -e "\e[93m not ${PWD}\e[0m" while $interactive; do read -p "Do you want to continue at current location? (yes/no) " yn - case $yn in - yes|Yes|y|Y|YES ) + case $yn in + yes|Yes|y|Y|YES ) break;; - no|No|n|N|NO ) + no|No|n|N|NO ) while true; do read -p "Do you want to change to \$WORKSPACE_ROOT/src and continue? (yes/no) " yn - case $yn in - yes|Yes|y|Y|YES ) + case $yn in + yes|Yes|y|Y|YES ) cd ${WORKSPACE_ROOT}/src if [[ ":$WORKSPACE_ROOT/src:" != *":$PWD"* ]]; then echo "Invalid folder ${PWD} - quit!" @@ -38,35 +50,35 @@ if [[ ":$WORKSPACE_ROOT/src:" != ":$PWD:" ]]; then fi echo "Continuing from ${PWD} ..." break;; - no|No|n|N|NO ) + no|No|n|N|NO ) echo -e "\e[93mChange to appropriate directory and retry!\e[0m"; exit;; - * ) echo invalid response;; + * ) + echo "invalid response" + exit;; esac - done - echo -e "\e[93mChange to appropriate directory and retry!\e[0m"; - exit;; - * ) echo invalid response;; + done;; + * ) + echo "invalid response" + exit;; esac done echo "" fi -name=$1 -echo -e "\e[96mInitializing new behaviors repo \e[1m${name}_behaviors ...\033[0m" echo "" echo -e "\e[96m(2/5) Fetching project structure...\033[0m" -git clone https://github.com/FlexBE/flexbe_project_behaviors.git ${name}_behaviors +git clone https://github.com/FlexBE/flexbe_project_behaviors.git ${meta_package_name} if [ ! $? -eq 0 ]; then echo -e "\e[92mFailed to clone the structure!\033[0m" exit fi -cd ${name}_behaviors +cd ${meta_package_name} echo -e "\e[96mSet up for ROS 2 development ...\033[0m" git fetch -git checkout ros2-devel # use new ROS 2 flexbe_app workspace layout, remove when merged into main +git checkout ros2-devel # use new ROS 2 flexbe_webui workspace layout, remove when merged into main echo "" echo -e "\e[96m(3/5) Configuring project template...\033[0m" @@ -97,20 +109,25 @@ sed -i -e "s/PROJECT_flexbe_behaviors/${name}_flexbe_behaviors/g" \ PROJECT_flexbe_behaviors/setup.py \ PROJECT_flexbe_behaviors/setup.cfg -sed -i -e "s/PROJECT/${name}/g" PROJECT_flexbe_behaviors/bin/copy_behavior +sed -i -e "s/PROJECT/${name}/g" PROJECT_flexbe_behaviors/bin/copy_behavior mv PROJECT_flexbe_behaviors/PROJECT_flexbe_behaviors PROJECT_flexbe_behaviors/${name}_flexbe_behaviors mv PROJECT_flexbe_behaviors/resource/PROJECT_flexbe_behaviors PROJECT_flexbe_behaviors/resource/${name}_flexbe_behaviors mv PROJECT_flexbe_behaviors ${name}_flexbe_behaviors # Handle the meta-package folder +sed -i -e "s/PROJECT_behaviors/${meta_package_name}/g" \ + PROJECT_behaviors/CMakeLists.txt \ + PROJECT_behaviors/package.xml \ + PROJECT_behaviors/CHANGELOG.rst sed -i -e "s/PROJECT/${name}/g" \ PROJECT_behaviors/CMakeLists.txt \ PROJECT_behaviors/package.xml \ PROJECT_behaviors/CHANGELOG.rst -mv PROJECT_behaviors${name}_behaviors +mv PROJECT_behaviors ${meta_package_name} +sed -i -e "s/PROJECT_behaviors/${meta_package_name}/g" README.md sed -i -e "s/PROJECT/${name}/g" README.md echo "" @@ -120,14 +137,14 @@ rm -rf .git while $interactive; do read -p "(5/5) Do you want to initialize a new Git repository for this project? (yes/no) " yn - case $yn in - yes|Yes|y|Y|YES ) + case $yn in + yes|Yes|y|Y|YES ) echo -e "\e[96m(5/5) Initializing new repository...\033[0m"; git init -b main git add . git commit -m "Initial commit" echo "" - echo -e "\e[92mCongratulations\033[0m, your new repository \e[1m${name}_behaviors\033[0m is ready to be pushed!" + echo -e "\e[92mCongratulations\033[0m, your new repository \e[1m${name}\033[0m is ready to be pushed!" echo -e "Please run the following commands to push it:" echo -e " git remote add origin [your_repo_url]" echo -e " git push origin main" @@ -141,7 +158,7 @@ done echo "" echo -e "Clone the following repository for a user interface (editor + runtime control) for FlexBE:" -echo -e " git clone https://github.com/FlexBE/flexbe_app.git" +echo -e " git clone https://github.com/FlexBE/flexbe_webui.git" echo "" echo -e "Consider running rosdep to update any missing dependencies ..." echo -e " rosdep install --from-paths src --ignore-src -r -y" diff --git a/flexbe_widget/bin/flexbe_status_listener b/flexbe_widget/bin/flexbe_status_listener new file mode 100755 index 0000000..8e7abc3 --- /dev/null +++ b/flexbe_widget/bin/flexbe_status_listener @@ -0,0 +1,6 @@ +#!/usr/bin/env python3 + +from flexbe_widget.flexbe_status_listener import flexbe_status_listener_main + +if __name__ == '__main__': + flexbe_status_listener_main() diff --git a/flexbe_widget/flexbe_widget/behavior_action_server.py b/flexbe_widget/flexbe_widget/behavior_action_server.py index ae4c79b..81054b7 100644 --- a/flexbe_widget/flexbe_widget/behavior_action_server.py +++ b/flexbe_widget/flexbe_widget/behavior_action_server.py @@ -45,7 +45,7 @@ from rosidl_runtime_py import get_interface_path -from std_msgs.msg import Empty, String +from std_msgs.msg import Empty, Int32 import yaml @@ -65,7 +65,7 @@ def __init__(self, node): 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(String, Topics._BEHAVIOR_UPDATE_TOPIC, self._state_cb, 100) + self._state_pub = self._node.create_subscription(Int32, Topics._BEHAVIOR_UPDATE_TOPIC, self._state_cb, 100) self._as = ActionServer(self._node, BehaviorExecution, Topics._EXECUTE_BEHAVIOR_ACTION, @@ -200,4 +200,4 @@ def _state_cb(self, msg): self._current_state = msg.data if self._current_goal and self._current_goal.is_active: self._current_goal.publish_feedback(BehaviorExecution.Feedback(current_state=self._current_state)) - self._node.get_logger().loginfo("Current state: '%s'" % self._current_state) + self._node.get_logger().loginfo('Current state id = %d' % self._current_state) diff --git a/flexbe_widget/flexbe_widget/behavior_launcher.py b/flexbe_widget/flexbe_widget/behavior_launcher.py index 85a643f..1072cc9 100644 --- a/flexbe_widget/flexbe_widget/behavior_launcher.py +++ b/flexbe_widget/flexbe_widget/behavior_launcher.py @@ -46,9 +46,11 @@ from flexbe_msgs.msg import BEStatus, BehaviorModification, BehaviorRequest from flexbe_msgs.msg import BehaviorSelection, BehaviorSync from flexbe_msgs.msg import ContainerStructure +from flexbe_msgs.msg import StateMapMsg import rclpy from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSProfile from rosidl_runtime_py import get_interface_path @@ -78,6 +80,10 @@ def __init__(self): self._mirror_pub = self.create_publisher(ContainerStructure, Topics._MIRROR_STRUCTURE_TOPIC, 100) self._heartbeat_pub = self.create_publisher(Int32, Topics._LAUNCHER_HEARTBEAT_TOPIC, 2) + # Latch state map so we can retrieve later if desired + latching_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + self._state_map_pub = self.create_publisher(StateMapMsg, Topics._STATE_MAP_OCS_TOPIC, latching_qos) + self._behavior_lib = BehaviorLibrary(self) # Require periodic events in case behavior is not connected to allow orderly shutdown @@ -120,10 +126,6 @@ def _request_callback(self, msg): if not self._ready_event.is_set(): Logger.logerr('Behavior engine is not ready - cannot process start request!') else: - # self._ready_event.clear() # require a new ready signal after publishing - # thread = threading.Thread(target=self._process_request, args=[msg]) - # thread.daemon = True - # thread.start() # Not waiting in process request, so safe to not block callback self._process_request(msg) @@ -166,13 +168,18 @@ def _process_request(self, msg): be_selection.arg_keys = msg.arg_keys be_selection.arg_values = msg.arg_values - container_map = StateMap() - be_structure = ContainerStructure() - be_structure.containers = msg.structure - for container in be_structure.containers: - # self.get_logger().info(f'BELauncher: request_callback: adding container {container.path} ...') - container_map.add_state(container.path, container) - # self.get_logger().info(f"BELauncher: request_callback: {msg.structure}") + state_map = StateMap() + try: + be_structure = ContainerStructure() + be_structure.containers = msg.structure + for container in be_structure.containers: + state_map.add_state(container.path, container) + self.get_logger().info(f'Built Statemachine {state_map}') + except Exception as exc: + self.get_logger().info(f"Failed to build state map for container {behavior['name']} ") + self.get_logger().info(f'{exc}') + self.get_logger().info(f'{state_map}') + raise exc try: be_filepath_new = self._behavior_lib.get_sourcecode_filepath(be_key) @@ -210,6 +217,17 @@ def _process_request(self, msg): new_content=content)) be_selection.behavior_id = zlib.adler32(be_content_new.encode()) & 0x7fffffff + + try: + state_ids, state_paths = list(zip(*state_map.items)) + state_map_msg = StateMapMsg(behavior_id=be_selection.behavior_id, + state_ids=state_ids, + state_paths=state_paths) + self._state_map_pub.publish(state_map_msg) # Used by the WebUI + + except Exception as exc: + self.get_logger().warn(f'Failed to publish state map from launcher!\n{exc}') + if msg.autonomy_level != 255: be_structure.behavior_id = be_selection.behavior_id self._mirror_pub.publish(be_structure) diff --git a/flexbe_widget/flexbe_widget/flexbe_status_listener.py b/flexbe_widget/flexbe_widget/flexbe_status_listener.py new file mode 100644 index 0000000..424f126 --- /dev/null +++ b/flexbe_widget/flexbe_widget/flexbe_status_listener.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from flexbe_core.core import StateMap, Topics + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSProfile + + +class FlexbeStatusListener(Node): + """Simple listener to echo FlexBE state machine information.""" + + def __init__(self): + super().__init__('flexbe_status_listener') + latching_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + self._state_map = None + self._state_map_sub = self.create_subscription( + Topics._topic_types[Topics._STATE_MAP_OCS_TOPIC], + Topics._STATE_MAP_OCS_TOPIC, + self._state_map_callback, + latching_qos) + self._state_map_sub # prevent unused variable warning + + self._outcome_sub = self.create_subscription( + Topics._topic_types[Topics._OUTCOME_TOPIC], + Topics._OUTCOME_TOPIC, + self._outcome_callback, + 10) + self._outcome_sub # prevent unused variable warning + + self._heartbeat_sub = self.create_subscription( + Topics._topic_types[Topics._ONBOARD_HEARTBEAT_TOPIC], + Topics._ONBOARD_HEARTBEAT_TOPIC, + self._heartbeat_callback, + 10) + self._heartbeat_sub # prevent unused variable warning + + self._sync_sub = self.create_subscription( + Topics._topic_types[Topics._MIRROR_SYNC_TOPIC], + Topics._MIRROR_SYNC_TOPIC, + self._sync_callback, + 10) + self._sync_sub # prevent unused variable warning + + def _heartbeat_callback(self, msg): + if self._state_map is None: + self.get_logger().info(f'Onboard heartbeat {msg}') + return + + states = [] + for checksum in msg.current_state_checksums: + state_id, _ = StateMap.unhash(checksum) + states.append(self._state_map.get(state_id, 'unknown')) + + self.get_logger().info(f'Onboard heartbeat {msg.behavior_id:10d} : {states}') + + def _sync_callback(self, msg): + if self._state_map is None: + self.get_logger().info(f'Synchronize mirror {msg}') + return + + states = [] + for checksum in msg.current_state_checksums: + state_id, _ = StateMap.unhash(checksum) + states.append(self._state_map.get(state_id, 'unknown')) + + self.get_logger().info(f'Synchronize mirror {msg.behavior_id:10d} : {states}') + + def _outcome_callback(self, msg): + if self._state_map is None: + self.get_logger().info(f'Outcome msg hash value {msg.data:11d}') + return + + state_id, outcome = StateMap.unhash(msg.data) + path = f"'{self._state_map.get(state_id, 'unknown')}'" + self.get_logger().info(f'Outcome {outcome:2d} from {state_id:11d} {path:60s}') + + def _state_map_callback(self, msg): + self.get_logger().info(f'New state map received for {msg.behavior_id}') + + state_map = {} + for id, path in zip(msg.state_ids, msg.state_paths): + self.get_logger().info(f" adding {id:11d} at '{path}'") + state_map[id] = path + + self._state_map = state_map + + +def flexbe_status_listener_main(args=None): + """Run the status listener to echo state info given state id in messages.""" + print('Suggest: export RCUTILS_CONSOLE_OUTPUT_FORMAT="[{time}] {message}" in terminal') + print(' Run this before starting behavior to get full state path information from state map.') + rclpy.init(args=args) + + outcomes = FlexbeStatusListener() + + rclpy.spin(outcomes) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + outcomes.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + flexbe_status_listener_main() diff --git a/flexbe_widget/package.xml b/flexbe_widget/package.xml index b8c5e0d..eea33f6 100644 --- a/flexbe_widget/package.xml +++ b/flexbe_widget/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> flexbe_widget - 3.0.6 + 4.0.1 flexbe_widget implements some smaller scripts for the behavior engine. diff --git a/flexbe_widget/setup.py b/flexbe_widget/setup.py index 25efcf2..7358a3b 100644 --- a/flexbe_widget/setup.py +++ b/flexbe_widget/setup.py @@ -38,7 +38,7 @@ setup( name=PACKAGE_NAME, - version='2.0.0', + version='4.0.1', packages=[PACKAGE_NAME], data_files=[ (os.path.join('share', PACKAGE_NAME), glob('launch/*.launch.py')),