From b1e92783a34c806c53e4b53100c49be62088a597 Mon Sep 17 00:00:00 2001 From: David Conner Date: Mon, 5 Aug 2024 15:07:00 -0400 Subject: [PATCH 01/23] 3.0.6 --- flexbe_behavior_engine/CHANGELOG.rst | 5 ++++- flexbe_behavior_engine/package.xml | 2 +- flexbe_core/CHANGELOG.rst | 15 +++++++++++++++ flexbe_core/package.xml | 2 +- flexbe_core/setup.py | 2 +- flexbe_input/CHANGELOG.rst | 5 +++++ flexbe_input/package.xml | 2 +- flexbe_input/setup.py | 2 +- flexbe_mirror/CHANGELOG.rst | 10 ++++++++++ flexbe_mirror/package.xml | 2 +- flexbe_mirror/setup.py | 2 +- flexbe_msgs/CHANGELOG.rst | 3 +++ flexbe_msgs/package.xml | 2 +- flexbe_onboard/CHANGELOG.rst | 6 ++++++ flexbe_onboard/package.xml | 2 +- flexbe_onboard/setup.py | 2 +- flexbe_states/CHANGELOG.rst | 6 ++++++ flexbe_states/package.xml | 2 +- flexbe_states/setup.py | 2 +- flexbe_testing/CHANGELOG.rst | 3 +++ flexbe_testing/package.xml | 2 +- flexbe_widget/CHANGELOG.rst | 3 +++ flexbe_widget/package.xml | 2 +- 23 files changed, 69 insertions(+), 15 deletions(-) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 6f1740a..4724867 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,10 +2,13 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -3.0.5 (2024-07-02) +3.0.6 (2024-08-05) ------------------ +3.0.5 (2024-07-02) +------------------ + 3.0.4 (2024-07-02) ------------------ diff --git a/flexbe_behavior_engine/package.xml b/flexbe_behavior_engine/package.xml index d345ff1..0d7842e 100644 --- a/flexbe_behavior_engine/package.xml +++ b/flexbe_behavior_engine/package.xml @@ -1,7 +1,7 @@ flexbe_behavior_engine - 3.0.5 + 3.0.6 A meta-package to aggregate all the FlexBE packages diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index 580fa19..c7f1003 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.6 (2024-08-05) +------------------ +* update cancel for action client +* define get_status to replace get_state for proxy action client +* remove result from input_state or cancel on exit +* call on_exit from state_machine and use that to handle concurrency container exit +* update get_required_autonomy check for concurrency container +* minor clean up from flake8/pycodestyle +* add option to globally set desired update rate by calling set_default_rate from behavior +* add peek_at_buffer to proxy subscriber +* modify processing of transition command for nested behaviors +* modify _entering flag and no sleep if entering +* grab reference to avoid race condition processing message in different thread while behavior finishing +* tweak state_map __str__ text. + 3.0.5 (2024-07-02) ------------------ diff --git a/flexbe_core/package.xml b/flexbe_core/package.xml index b282571..dbc72c4 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.5 + 3.0.6 flexbe_core provides the core components for the FlexBE behavior engine. diff --git a/flexbe_core/setup.py b/flexbe_core/setup.py index 0a7abbf..9010ba3 100644 --- a/flexbe_core/setup.py +++ b/flexbe_core/setup.py @@ -6,7 +6,7 @@ setup( name=PACKAGE_NAME, - version='3.0.5', + version='3.0.6', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]), diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index fd86aa9..7abb614 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.6 (2024-08-05) +------------------ +* allow canceling input request +* set style for the Input GUI + 3.0.5 (2024-07-02) ------------------ diff --git a/flexbe_input/package.xml b/flexbe_input/package.xml index 1b82393..91a882b 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.5 + 3.0.6 flexbe_input enables to send data to onboard behavior when required. diff --git a/flexbe_input/setup.py b/flexbe_input/setup.py index 521abe2..5560410 100644 --- a/flexbe_input/setup.py +++ b/flexbe_input/setup.py @@ -6,7 +6,7 @@ setup( name=PACKAGE_NAME, - version='3.0.5', + version='3.0.6', 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 f6a620c..d9b36ac 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.6 (2024-08-05) +------------------ +* clean up sync messaging and set entering flag for state +* minor clean up from flake8/pycodestyle +* fix typo in flexbe_mirror +* ignore second behavior start request while first is processing (in case of zombie launcher) +* fix state map id verification with collisions +* rework re-request structure logic if behavior sm fails during mirroring to avoid spamming console +* clean up some messages + 3.0.5 (2024-07-02) ------------------ diff --git a/flexbe_mirror/package.xml b/flexbe_mirror/package.xml index d216511..e6ddc26 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.5 + 3.0.6 flexbe_mirror implements functionality to remotely mirror an executed behavior. diff --git a/flexbe_mirror/setup.py b/flexbe_mirror/setup.py index 6812819..e2473e7 100644 --- a/flexbe_mirror/setup.py +++ b/flexbe_mirror/setup.py @@ -7,7 +7,7 @@ setup( name=PACKAGE_NAME, - version='3.0.5', + version='3.0.6', 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 46d98f0..30f99eb 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.6 (2024-08-05) +------------------ + 3.0.5 (2024-07-02) ------------------ diff --git a/flexbe_msgs/package.xml b/flexbe_msgs/package.xml index be25e67..3d02a40 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.5 + 3.0.6 flexbe_msgs provides the messages used by FlexBE. diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index 5125726..dc60854 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.6 (2024-08-05) +------------------ +* minor clean up from flake8/pycodestyle +* fix typo in flexbe_mirror +* ignore second behavior start request while first is processing (in case of zombie launcher) + 3.0.5 (2024-07-02) ------------------ diff --git a/flexbe_onboard/package.xml b/flexbe_onboard/package.xml index 9523483..8068892 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.5 + 3.0.6 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 c856525..a25463c 100644 --- a/flexbe_onboard/setup.py +++ b/flexbe_onboard/setup.py @@ -9,7 +9,7 @@ setup( name=package_name, - version='3.0.5', + version='3.0.6', packages=find_packages(), data_files=[ (os.path.join('share', package_name), glob('launch/*.launch.py')), diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index ad415d0..e94bf5c 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.6 (2024-08-05) +------------------ +* update cancel for action client; define get_status to replace get_state for proxy action client +* remove result from input_state or cancel on exit +* tweak help description of calculation/condition states + 3.0.5 (2024-07-02) ------------------ diff --git a/flexbe_states/package.xml b/flexbe_states/package.xml index 46e0239..09970b8 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.5 + 3.0.6 flexbe_states provides a collection of common generic predefined states. diff --git a/flexbe_states/setup.py b/flexbe_states/setup.py index 65ef1ad..a7a927a 100644 --- a/flexbe_states/setup.py +++ b/flexbe_states/setup.py @@ -9,7 +9,7 @@ setup( name=PACKAGE_NAME, - version='3.0.5', + version='3.0.6', 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 70f7885..1d9be5a 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.6 (2024-08-05) +------------------ + 3.0.5 (2024-07-02) ------------------ diff --git a/flexbe_testing/package.xml b/flexbe_testing/package.xml index 835f2c9..918c2bc 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.5 + 3.0.6 flexbe_testing provides a framework for unit testing states. diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index 36c489c..c4c9d2d 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.6 (2024-08-05) +------------------ + 3.0.5 (2024-07-02) ------------------ diff --git a/flexbe_widget/package.xml b/flexbe_widget/package.xml index afe01c1..b8c5e0d 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.5 + 3.0.6 flexbe_widget implements some smaller scripts for the behavior engine. From 2010d6164a4b4ddf07da8d77c977a52f574e3a72 Mon Sep 17 00:00:00 2001 From: David Conner Date: Thu, 8 Aug 2024 17:43:20 -0400 Subject: [PATCH 02/23] updates to ConcurrencyContainer and StateMachine to handle sync and forced outcomes --- .../flexbe_core/core/concurrency_container.py | 23 ++++++++++--------- .../core/operatable_state_machine.py | 23 +++++++++++++++++++ flexbe_core/flexbe_core/core/state_machine.py | 1 + flexbe_mirror/flexbe_mirror/flexbe_mirror.py | 6 +++++ .../mirror_concurrency_container.py | 3 ++- flexbe_mirror/flexbe_mirror/mirror_state.py | 3 ++- .../flexbe_mirror/mirror_state_machine.py | 21 +++++++++++++++-- 7 files changed, 65 insertions(+), 15 deletions(-) diff --git a/flexbe_core/flexbe_core/core/concurrency_container.py b/flexbe_core/flexbe_core/core/concurrency_container.py index 76c76f3..c8d446a 100644 --- a/flexbe_core/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/flexbe_core/core/concurrency_container.py @@ -86,7 +86,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}') @@ -95,20 +95,21 @@ def get_required_autonomy(self, outcome, state): def _execute_current_state(self): """Execute the current states within this concurrency container.""" # execute all states that are done with sleeping and determine next sleep duration + if self._entering: + self.on_enter(self._userdata) + self._inner_sync_request = False # clear prior request for lower level state 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: 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.name}' is handling the transition cmd msg={command_msg}") self._force_transition = True outcome = self.outcomes[command_msg.outcome] @@ -117,19 +118,19 @@ def _execute_current_state(self): CommandFeedback(command='transition', args=[command_msg.target, self.name])) Logger.localwarn(f"--> Manually triggered outcome {outcome} of concurrency container '{self.name}'") + 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 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: @@ -139,7 +140,7 @@ def _execute_current_state(self): 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.name}' is handling " f"the cmd msg='{command_msg}'") self._manual_transition_requested = None # Reset at this level @@ -150,7 +151,7 @@ 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 '{outcome}' and on exit for '{state}'") state.on_exit(userdata) # ConcurrencyContainer bypasses normal operatable state handling of manual request, so do that here @@ -160,7 +161,7 @@ def _execute_current_state(self): CommandFeedback(command='transition', args=[command_msg.target, state.name])) 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}' " @@ -270,10 +271,10 @@ 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: 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 = {} diff --git a/flexbe_core/flexbe_core/core/operatable_state_machine.py b/flexbe_core/flexbe_core/core/operatable_state_machine.py index d074a8f..e92ee2d 100644 --- a/flexbe_core/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/flexbe_core/core/operatable_state_machine.py @@ -180,6 +180,29 @@ def get_latest_status(self): # execution def _execute_current_state(self): + + 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.name: + 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}' 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 statemachine '{self.name}'") + self._last_outcome = outcome + self._publish_outcome(outcome) + + return outcome + # catch any exception and keep state active to let operator intervene try: # --- @TODO remove self._inner_sync_request = False # clear any prior sync request diff --git a/flexbe_core/flexbe_core/core/state_machine.py b/flexbe_core/flexbe_core/core/state_machine.py index 14f806f..fb43547 100644 --- a/flexbe_core/flexbe_core/core/state_machine.py +++ b/flexbe_core/flexbe_core/core/state_machine.py @@ -128,6 +128,7 @@ def execute(self, userdata): self.assert_consistent_transitions() self._entering = 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}' " diff --git a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py index b88ff28..cda2b90 100644 --- a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py @@ -460,6 +460,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(f'\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}') diff --git a/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py b/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py index 8d3b580..9903751 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py +++ b/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py @@ -69,7 +69,8 @@ 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: + self._last_outcome = self.outcomes[desired_outcome] return self._last_outcome def execute_mirror(self, userdata): diff --git a/flexbe_mirror/flexbe_mirror/mirror_state.py b/flexbe_mirror/flexbe_mirror/mirror_state.py index 3484daf..d6b7d9d 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_state.py +++ b/flexbe_mirror/flexbe_mirror/mirror_state.py @@ -102,7 +102,8 @@ def on_enter_mirror(self, userdata): def on_exit_mirror(self, userdata, desired_outcome): """Exit mirror state.""" try: - self._last_outcome = self.outcomes[desired_outcome] + if desired_outcome != -1: + 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..b6f66fd 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_state_machine.py +++ b/flexbe_mirror/flexbe_mirror/mirror_state_machine.py @@ -73,6 +73,7 @@ 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 @@ -184,6 +185,21 @@ def _execute_current_state_mirror(self, userdata): # Current state might be None while waiting on final outcome message to exit SM return None + if MirrorState._last_state_id == self.state_id: + # Handle this state machine outcome + if self._last_outcome is not None: + Logger.localwarn(f"Already processed outcome='{self._last_outcome}' for " + f" state '{self.name}' ({self.state_id}) given new " + f'outcome index={MirrorState._last_state_outcome}') + + MirrorState._last_state_id = None # Flag that the message was handled + if MirrorState._last_state_outcome is not None: + Logger.localwarn(f" StateMachine '{self.name}' ({self.state_id}) processing " + f'outcome index={MirrorState._last_state_outcome}') + outcome = self.on_exit_mirror(userdata, MirrorState._last_state_outcome) + MirrorState._last_state_outcome = None # Flag that the message was handled + return outcome + # Process the current state outcome = self._current_state.execute_mirror(userdata) if outcome is not None: @@ -278,8 +294,9 @@ def on_exit_mirror(self, userdata, desired_outcome=-1): 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: + self._last_outcome = self.outcomes[desired_outcome] self._current_state = None self._entering = True return self._last_outcome From 7e5efb7901efd2a1b66ae82ba7a66d83427363a6 Mon Sep 17 00:00:00 2001 From: David Conner Date: Thu, 8 Aug 2024 18:49:37 -0400 Subject: [PATCH 03/23] add initialize_flexbe_core for common initialization --- flexbe_core/flexbe_core/__init__.py | 21 ++++++++++++--- .../flexbe_core/core/concurrency_container.py | 3 ++- .../flexbe_core/proxy/proxy_action_client.py | 1 + flexbe_core/test/test_core.py | 20 ++++---------- flexbe_core/test/test_exceptions.py | 24 +++-------------- flexbe_core/test/test_logger.py | 27 +++++-------------- .../flexbe_input/input_action_server.py | 15 +++++++---- flexbe_input/flexbe_input/input_gui.py | 12 +++++---- flexbe_mirror/flexbe_mirror/flexbe_mirror.py | 16 ++++------- .../complex_behavior_test_sm.py | 9 ++----- .../log_behavior_test_sm.py | 6 ++--- flexbe_onboard/tests/test_onboard.py | 12 ++++++--- flexbe_states/flexbe_states/input_state.py | 1 + .../ros2-conversion-best-practices.md | 13 +++++++-- .../test/selftest_behavior_sm.py | 6 ++--- 15 files changed, 84 insertions(+), 102 deletions(-) diff --git a/flexbe_core/flexbe_core/__init__.py b/flexbe_core/flexbe_core/__init__.py index 32ef254..d1d92f2 100644 --- a/flexbe_core/flexbe_core/__init__.py +++ b/flexbe_core/flexbe_core/__init__.py @@ -48,17 +48,29 @@ # 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/core/concurrency_container.py b/flexbe_core/flexbe_core/core/concurrency_container.py index c8d446a..c32a0eb 100644 --- a/flexbe_core/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/flexbe_core/core/concurrency_container.py @@ -151,7 +151,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"ConcurrencyContainer '{self}' manual transition '{outcome}' 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 diff --git a/flexbe_core/flexbe_core/proxy/proxy_action_client.py b/flexbe_core/flexbe_core/proxy/proxy_action_client.py index 0116399..62f1b8c 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 diff --git a/flexbe_core/test/test_core.py b/flexbe_core/test/test_core.py index 660cbf7..f8ad0a6 100755 --- a/flexbe_core/test/test_core.py +++ b/flexbe_core/test/test_core.py @@ -33,11 +33,11 @@ import time import unittest -from flexbe_core import ConcurrencyContainer, EventState, OperatableStateMachine +from flexbe_core import ConcurrencyContainer, EventState, OperatableStateMachine, initialize_flexbe_core 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.proxy import ProxySubscriberCached, shutdown_proxies from flexbe_msgs.msg import CommandFeedback, OutcomeRequest @@ -132,7 +132,7 @@ 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) @@ -160,8 +160,7 @@ def tearDown(self): 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']) @@ -230,7 +229,6 @@ def test_event_state(self): 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) state, sm = self._create() @@ -287,7 +285,6 @@ def test_operatable_state(self): self.node.get_logger().info('test_operatable_state ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - ProxySubscriberCached.initialize(self.node) state, sm = self._create() self.node.get_logger().info('test_operatable_state - ProxySubscribe request ...') out_topic = Topics._OUTCOME_TOPIC @@ -343,7 +340,6 @@ def test_preemptable_state(self): self.node.get_logger().info('test_preemptable_state ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - ProxySubscriberCached.initialize(self.node) state, sm = self._create() fb_topic = Topics._CMD_FEEDBACK_TOPIC @@ -386,7 +382,6 @@ def test_lockable_state(self): self.node.get_logger().info('test_lockable_state ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - ProxySubscriberCached.initialize(self.node) state, sm = self._create() fb_topic = Topics._CMD_FEEDBACK_TOPIC sub = ProxySubscriberCached({fb_topic: CommandFeedback}, inst_id=id(self)) @@ -460,7 +455,6 @@ def test_manually_transitionable_state(self): self.node.get_logger().info('test_manually_transitionable_state ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - ProxySubscriberCached.initialize(self.node) state, sm = self._create() fb_topic = Topics._CMD_FEEDBACK_TOPIC sub = ProxySubscriberCached({fb_topic: CommandFeedback}, inst_id=id(self)) @@ -476,7 +470,7 @@ def test_manually_transitionable_state(self): state._sub._callback(OutcomeRequest(target='invalid', 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! ') @@ -541,8 +535,6 @@ 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) cc = ConcurrencyContainer(outcomes=['done', 'error'], conditions=[ ('error', [('main', 'error')]), @@ -692,8 +684,6 @@ 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' diff --git a/flexbe_core/test/test_exceptions.py b/flexbe_core/test/test_exceptions.py index d5d27d4..03a7d98 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 @@ -60,7 +60,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.""" @@ -87,14 +87,11 @@ def test_invalid_outcome(self): 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 class ReturnInvalidOutcomeState(EventState): """Local Test state definition.""" def __init__(self): - self.initialize_ros(node) super().__init__(outcomes=['done']) def execute(self, userdata): @@ -114,14 +111,11 @@ def test_invalid_transition(self): 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 class ReturnDoneState(EventState): """Local Test state definition.""" def __init__(self): - ReturnDoneState.initialize_ros(node) super().__init__(outcomes=['done']) def execute(self, userdata): @@ -144,14 +138,11 @@ def test_invalid_userdata_input(self): 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 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): @@ -172,14 +163,11 @@ def test_invalid_userdata_output(self): 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 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): @@ -200,14 +188,11 @@ def test_missing_userdata(self): 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 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): @@ -228,14 +213,11 @@ def test_modify_input_key(self): 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 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): diff --git a/flexbe_core/test/test_logger.py b/flexbe_core/test/test_logger.py index 225cf99..49a93f5 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 @@ -55,12 +55,11 @@ 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.""" @@ -87,17 +86,14 @@ def test_throttle_logger_one(self): 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 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') @@ -130,17 +126,14 @@ def test_throttle_logger_err_multi(self): 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 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 +167,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 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') @@ -222,17 +212,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 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') diff --git a/flexbe_input/flexbe_input/input_action_server.py b/flexbe_input/flexbe_input/input_action_server.py index 5f3fdfb..63a05bf 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,6 +48,8 @@ class InputActionWorker(QThread): + """Worker thread for InputAction server.""" + _show_dialog_signal = Signal(str) _hide_dialog_signal = Signal() @@ -56,6 +58,7 @@ 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() @@ -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 @@ -192,12 +195,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 +233,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..d2c0794 100644 --- a/flexbe_input/flexbe_input/input_gui.py +++ b/flexbe_input/flexbe_input/input_gui.py @@ -55,13 +55,13 @@ def __init__(self, prompt): central_widget = QWidget(self) self.setCentralWidget(central_widget) - central_widget.setStyleSheet("QWidget { border: 1px solid blue; background-color: palette(window); }") + central_widget.setStyleSheet('QWidget { border: 1px solid blue; background-color: palette(window); }') layout = QVBoxLayout(central_widget) self.prompt = QLabel(self) self.prompt.setText(prompt) - self.prompt.setStyleSheet("QLabel { border: none; background-color: palette(window); }") + self.prompt.setStyleSheet('QLabel { border: none; background-color: palette(window); }') layout.addWidget(self.prompt) edit_style = """ @@ -120,10 +120,11 @@ def set_cancel(self): @Slot(str) def show(self, prompt): - print(f"showing dialog with '{prompt}' ", flush=True) + """Show dialog if hidden.""" + print(f"showing input UI dialog with '{prompt}' ", flush=True) self.prompt.setText(prompt) self.prompt.adjustSize() - self.line.setText("") + self.line.setText('') self.input = None # clear for next entry self.adjustSize() self.resize(self.sizeHint()) # Resize to fit the new content @@ -131,7 +132,8 @@ def show(self, prompt): @Slot() def hide(self): - print("hiding dialog", flush=True) + """Hide dialog when not in use.""" + print('hiding input UI dialog', flush=True) super().hide() def is_none(self): diff --git a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py index cda2b90..3543deb 100644 --- a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py @@ -42,7 +42,7 @@ 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.topics import Topics @@ -82,14 +82,7 @@ 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 @@ -462,7 +455,7 @@ def _sync_callback(self, msg): thread.start() # Force a new update after sync - Logger.localinfo(f'\x1b[93mReceived sync for current behavior - request behavior update message\x1b[0m') + 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 @@ -517,7 +510,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}') 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..858b5b4 100755 --- a/flexbe_onboard/tests/test_onboard.py +++ b/flexbe_onboard/tests/test_onboard.py @@ -225,12 +225,18 @@ def test_onboard_behaviors(self): # Wait for published message end_time = time.time() + 1 - while time.time() < end_time: - self.executor.spin_once(timeout_sec=0.1) + 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.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/flexbe_states/input_state.py b/flexbe_states/flexbe_states/input_state.py index 77b2ed9..44f6489 100644 --- a/flexbe_states/flexbe_states/input_state.py +++ b/flexbe_states/flexbe_states/input_state.py @@ -147,6 +147,7 @@ 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}'.") 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_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 From 3c83ec966ca911c81d820e73b181ac47e3a6cca4 Mon Sep 17 00:00:00 2001 From: David Conner Date: Fri, 16 Aug 2024 00:16:21 -0400 Subject: [PATCH 04/23] add new states; modify BehaviorInput to allow strings and selection combo box --- .../flexbe_input/input_action_server.py | 24 ++- flexbe_input/flexbe_input/input_gui.py | 148 +++++++++++++---- flexbe_msgs/action/BehaviorInput.action | 6 +- flexbe_states/flexbe_states/input_state.py | 28 ++-- flexbe_states/flexbe_states/log_key_state.py | 8 +- .../flexbe_states/selection_state.py | 153 ++++++++++++++++++ .../flexbe_states/user_data_state.py | 56 +++++++ 7 files changed, 370 insertions(+), 53 deletions(-) create mode 100644 flexbe_states/flexbe_states/selection_state.py create mode 100644 flexbe_states/flexbe_states/user_data_state.py diff --git a/flexbe_input/flexbe_input/input_action_server.py b/flexbe_input/flexbe_input/input_action_server.py index 63a05bf..9087f69 100644 --- a/flexbe_input/flexbe_input/input_action_server.py +++ b/flexbe_input/flexbe_input/input_action_server.py @@ -50,7 +50,7 @@ class InputActionWorker(QThread): """Worker thread for InputAction server.""" - _show_dialog_signal = Signal(str) + _show_dialog_signal = Signal(str, object) _hide_dialog_signal = Signal() def __init__(self, node): @@ -116,6 +116,8 @@ def get_input_type(self, request_type): 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: @@ -144,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 @@ -166,15 +171,21 @@ 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}'" + if type_class is str: + result.data = self._input + data_len = 1 + else: + input_data = ast.literal_eval(self._input) # convert string to Python data + result.data = str(pickle.dumps(input_data)) + data_len = 1 if isinstance(input_data, (int, float)) else len(input_data) + + if not isinstance(result.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 - 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}'") @@ -182,7 +193,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 diff --git a/flexbe_input/flexbe_input/input_gui.py b/flexbe_input/flexbe_input/input_gui.py index d2c0794..e3b17b9 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 + 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 layout + self.clear_layout() self.prompt = QLabel(self) - self.prompt.setText(prompt) self.prompt.setStyleSheet('QLabel { border: none; background-color: palette(window); }') - layout.addWidget(self.prompt) + self.prompt.setText(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,33 +162,54 @@ 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): + def show(self, prompt, items=None): """Show dialog if hidden.""" print(f"showing input UI dialog with '{prompt}' ", flush=True) - self.prompt.setText(prompt) - self.prompt.adjustSize() - self.line.setText('') 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() @@ -134,6 +217,7 @@ def show(self, prompt): def hide(self): """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_msgs/action/BehaviorInput.action b/flexbe_msgs/action/BehaviorInput.action index 8231282..74b42a8 100644 --- a/flexbe_msgs/action/BehaviorInput.action +++ b/flexbe_msgs/action/BehaviorInput.action @@ -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_states/flexbe_states/input_state.py b/flexbe_states/flexbe_states/input_state.py index 44f6489..85e74f3 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 @@ -152,10 +157,11 @@ def on_exit(self, userdata): # 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..c52b803 --- /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 + + # Retrive 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' From e5adf83a5336d18d61af0f75bbf3eb672136eca1 Mon Sep 17 00:00:00 2001 From: David Conner Date: Fri, 16 Aug 2024 00:19:34 -0400 Subject: [PATCH 05/23] modify clear action handling; retain action result status; reduce startup spam --- .../flexbe_core/proxy/proxy_action_client.py | 36 +++++++++++++++++-- .../proxy/proxy_subscriber_cached.py | 6 ++-- 2 files changed, 36 insertions(+), 6 deletions(-) diff --git a/flexbe_core/flexbe_core/proxy/proxy_action_client.py b/flexbe_core/flexbe_core/proxy/proxy_action_client.py index 62f1b8c..d7e8989 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_action_client.py +++ b/flexbe_core/flexbe_core/proxy/proxy_action_client.py @@ -52,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.""" @@ -267,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 @@ -280,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): @@ -345,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): """ @@ -371,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_subscriber_cached.py b/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py index 77765bc..6e4b28c 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py +++ b/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py @@ -143,9 +143,9 @@ 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" From 20438399192e471914d1ec9c38357af0bfe1d81b Mon Sep 17 00:00:00 2001 From: David Conner Date: Fri, 16 Aug 2024 00:21:01 -0400 Subject: [PATCH 06/23] allow controllable OSM and concurrency outputs; improve sync handling; unhandled state exception stops behavior --- .../flexbe_core/core/concurrency_container.py | 21 +- .../flexbe_core/core/operatable_state.py | 6 +- .../core/operatable_state_machine.py | 46 +++- .../core/preemptable_state_machine.py | 14 +- flexbe_core/flexbe_core/core/state_machine.py | 31 ++- .../test/flexbe_exceptions_spin_test.py | 70 +++++ flexbe_core/test/test_exceptions.py | 47 +++- flexbe_core/test/test_exceptions_spin.py | 251 ++++++++++++++++++ flexbe_core/test/test_proxies.py | 4 +- 9 files changed, 444 insertions(+), 46 deletions(-) create mode 100644 flexbe_core/test/flexbe_exceptions_spin_test.py create mode 100644 flexbe_core/test/test_exceptions_spin.py diff --git a/flexbe_core/flexbe_core/core/concurrency_container.py b/flexbe_core/flexbe_core/core/concurrency_container.py index c32a0eb..0d8d501 100644 --- a/flexbe_core/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/flexbe_core/core/concurrency_container.py @@ -96,6 +96,7 @@ def _execute_current_state(self): """Execute the current states within this concurrency container.""" # execute all states that are done with sleeping and determine next sleep duration if self._entering: + Logger.localerr(f"CC: Why is entering flag set here '{self.name}' of '{self.path}'?") self.on_enter(self._userdata) self._inner_sync_request = False # clear prior request for lower level state @@ -128,6 +129,12 @@ def _execute_current_state(self): 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.localinfo(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) @@ -213,11 +220,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) @@ -226,7 +228,7 @@ def _execute_current_state(self): 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)) + 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)) @@ -234,6 +236,7 @@ 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 + Logger.localinfo(f"controlled CC '{self.name}' from '{self.path}'permitting outcome '{outcome}' ") self._publish_outcome(outcome) self._force_transition = False @@ -263,6 +266,7 @@ def _execute_single_state(self, state, force_exit=False): def on_enter(self, userdata): # pylint: disable=W0613 """Call on entering the concurrency container.""" + Logger.localinfo(f" CC on_enter for '{self.path}' ... ") super().on_enter(userdata) self._returned_outcomes = {} for state in self._states: @@ -281,6 +285,11 @@ def on_exit(self, userdata, states=None): self._returned_outcomes = {} self._entering = True + 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.path)) + self._last_requested_outcome = None + def get_deep_states(self): """ Recursively look for the currently executing states. diff --git a/flexbe_core/flexbe_core/core/operatable_state.py b/flexbe_core/flexbe_core/core/operatable_state.py index 357e70b..c724d6e 100644 --- a/flexbe_core/flexbe_core/core/operatable_state.py +++ b/flexbe_core/flexbe_core/core/operatable_state.py @@ -64,7 +64,7 @@ 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._last_requested_outcome = None @@ -75,7 +75,7 @@ def _operatable_execute(self, *args, **kwargs): 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)) + 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,8 +84,10 @@ 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: + Logger.localinfo(f"controlled State '{self.name}' from '{self.path}'permitting outcome '{outcome}' ") self._publish_outcome(outcome) self._force_transition = False + return outcome def _publish_outcome(self, outcome): diff --git a/flexbe_core/flexbe_core/core/operatable_state_machine.py b/flexbe_core/flexbe_core/core/operatable_state_machine.py index e92ee2d..3a37d38 100644 --- a/flexbe_core/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/flexbe_core/core/operatable_state_machine.py @@ -181,6 +181,11 @@ def get_latest_status(self): # execution def _execute_current_state(self): + if self._entering: + # On entering this state machine + Logger.localerr(f"OSM: Why is entering flag set here '{self.name}' of '{self.path}'?") + raise Exception("entering flag still set L187 of OSM - '{self.path}'") + self._manual_transition_requested = None if self._is_controlled and self._sub.has_buffered(Topics._CMD_TRANSITION_TOPIC): # Special handling in statemachine container @@ -189,7 +194,8 @@ def _execute_current_state(self): if command_msg.target == self.name: 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}' is handling the transition cmd msg={command_msg}") + 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] @@ -200,28 +206,27 @@ def _execute_current_state(self): Logger.localwarn(f"--> Manually triggered outcome {outcome} of statemachine '{self.name}'") self._last_outcome = outcome self._publish_outcome(outcome) - return outcome - # catch any exception and keep state active to let operator intervene + 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.localinfo(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 + # catch any exception and log here, but re-raise to stop behavior outcome = None self._last_exception = exc - Logger.logerr('Failed to execute state %s:\n%s' % (self.current_state_label, str(exc))) + Logger.logerr("Failed to execute state '%s':\n%s" % (self.current_state_label, str(exc))) import traceback # pylint: disable=C0415 Logger.localinfo(traceback.format_exc().replace('%', '%%')) # Guard against exeception including format! + raise exc 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 @@ -231,15 +236,15 @@ def _execute_current_state(self): 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)) + 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: + if outcome is not None and outcome in self.outcomes: + Logger.localinfo(f"controlled SM '{self.name}' from '{self.path}'returning outcome '{outcome}' ") self._publish_outcome(outcome) self._force_transition = False @@ -439,8 +444,16 @@ 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.""" + Logger.localinfo(f"OSM on enter for '{self.name}' from '{self.path}' ...") + self._last_exception = None + self._last_requested_outcome = None + super().on_enter(userdata) + def on_exit(self, userdata=None): """Call on exiting the statemachine.""" + Logger.localinfo(f"SM on exit for '{self.name}' from '{self.path}' ...") self._entering = True if self._current_state is not None: with UserData(reference=self._userdata, @@ -451,3 +464,8 @@ def on_exit(self, userdata=None): self._current_state.on_exit(udata) self._current_state._entering = True self._current_state = None + + 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.path)) + self._last_requested_outcome = None diff --git a/flexbe_core/flexbe_core/core/preemptable_state_machine.py b/flexbe_core/flexbe_core/core/preemptable_state_machine.py index dacabe3..86f4318 100644 --- a/flexbe_core/flexbe_core/core/preemptable_state_machine.py +++ b/flexbe_core/flexbe_core/core/preemptable_state_machine.py @@ -82,13 +82,20 @@ def add(label, state, transitions=None, remapping=None): def _valid_targets(self): return super()._valid_targets + [PreemptableStateMachine._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) @@ -122,7 +129,6 @@ def spin(self, userdata=None): break self.wait(seconds=self.sleep_duration) - return outcome def get_latest_status(self): diff --git a/flexbe_core/flexbe_core/core/state_machine.py b/flexbe_core/flexbe_core/core/state_machine.py index fb43547..013eda3 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,15 +131,9 @@ 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._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}' " - # f"({self._state_id}) initial state='{self._current_state.name}'") + if self._entering: + self.on_enter(userdata) + outcome = self._execute_current_state() if outcome: @@ -141,6 +142,16 @@ def execute(self, userdata): return outcome + def on_enter(self, userdata): + self.assert_consistent_transitions() + self._entering = 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}'") + 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], 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/test_exceptions.py b/flexbe_core/test/test_exceptions.py index 03a7d98..63432b9 100755 --- a/flexbe_core/test/test_exceptions.py +++ b/flexbe_core/test/test_exceptions.py @@ -97,11 +97,15 @@ def __init__(self): 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'}) + 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! ') @@ -128,7 +132,12 @@ def execute(self, userdata): 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! ') @@ -153,7 +162,12 @@ def execute(self, userdata): with sm: OperatableStateMachine.add('state', AccessInvalidInputState(), 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 - OK! ') @@ -178,7 +192,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! ') @@ -203,7 +222,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! ') @@ -229,7 +253,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..adde1f5 --- /dev/null +++ b/flexbe_core/test/test_exceptions_spin.py @@ -0,0 +1,251 @@ +#!/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 + + 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=0.1) + + self.node.get_logger().info(' shutting down proxies in core test %d ... ' % (self.test)) + shutdown_proxies() + time.sleep(0.1) + + self.node.get_logger().info(' destroy node in core test %d ... ' % (self.test)) + self.node.destroy_node() + + time.sleep(0.1) + self.executor.shutdown() + time.sleep(0.1) + + # Kill it with fire to make sure not stray published topics are available + rclpy.shutdown(context=self.context) + time.sleep(0.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) + + class ReturnInvalidOutcomeState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done']) + + 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']) + 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=1) + + class ReturnDoneState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done']) + + def execute(self, userdata): + return 'done' + + inner_sm = OperatableStateMachine(outcomes=['done']) + with inner_sm: + OperatableStateMachine.add('state', ReturnDoneState(), transitions={'done': 'invalid'}) + sm = OperatableStateMachine(name='test_invalid_transition', outcomes=['done']) + 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=1) + + class AccessInvalidInputState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done'], input_keys=['input']) + + def execute(self, userdata): + print(userdata.invalid) + return 'done' + + sm = OperatableStateMachine(name='test_invalid_userdata_input', outcomes=['done']) + 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=1) + + class SetInvalidOutputState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done'], output_keys=['output']) + + def execute(self, userdata): + userdata.invalid = False + return 'done' + + sm = OperatableStateMachine(name='test_invalid_userdata_output', outcomes=['done']) + 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=1) + + class AccessValidInputState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done'], input_keys=['missing']) + + def execute(self, userdata): + print(userdata.missing) + return 'done' + + sm = OperatableStateMachine(name='test_missing_userdata', outcomes=['done']) + 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=1) + + class ModifyInputKeyState(EventState): + """Local Test state definition.""" + + def __init__(self): + super().__init__(outcomes=['done'], input_keys=['only_input']) + + def execute(self, userdata): + userdata.only_input['new'] = 'not_allowed' + return 'done' + + sm = OperatableStateMachine(name='test_modify_input_key', outcomes=['done']) + 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_proxies.py b/flexbe_core/test/test_proxies.py index 0482a32..49f801a 100755 --- a/flexbe_core/test/test_proxies.py +++ b/flexbe_core/test/test_proxies.py @@ -333,7 +333,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! ') From 23211bd3d2ef62c252661aa0ca189c5c207228e2 Mon Sep 17 00:00:00 2001 From: David Conner Date: Fri, 16 Aug 2024 00:30:58 -0400 Subject: [PATCH 07/23] modify mirror handling for controllable OSM/CC and improve sync --- flexbe_mirror/flexbe_mirror/flexbe_mirror.py | 2 +- .../mirror_concurrency_container.py | 7 ++- .../flexbe_mirror/mirror_state_machine.py | 43 +++++++------------ 3 files changed, 23 insertions(+), 29 deletions(-) diff --git a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py index 3543deb..c6a5881 100644 --- a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py @@ -755,7 +755,7 @@ def _execute_mirror(self, start_time): result = PreemptableStateMachine._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: diff --git a/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py b/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py index 9903751..b47201f 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py +++ b/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py @@ -85,7 +85,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') @@ -114,6 +115,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._target_path) # Notify back at top-level before exit return None def get_deep_states(self): diff --git a/flexbe_mirror/flexbe_mirror/mirror_state_machine.py b/flexbe_mirror/flexbe_mirror/mirror_state_machine.py index b6f66fd..6d7f2dc 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_state_machine.py +++ b/flexbe_mirror/flexbe_mirror/mirror_state_machine.py @@ -59,11 +59,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 @@ -92,7 +93,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 @@ -100,9 +102,10 @@ 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}') @@ -111,8 +114,8 @@ def spin(self, start_time, userdata=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._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 @@ -137,11 +140,6 @@ 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}" ' - wait for confirming top-level outcome message!') @@ -185,21 +183,6 @@ def _execute_current_state_mirror(self, userdata): # Current state might be None while waiting on final outcome message to exit SM return None - if MirrorState._last_state_id == self.state_id: - # Handle this state machine outcome - if self._last_outcome is not None: - Logger.localwarn(f"Already processed outcome='{self._last_outcome}' for " - f" state '{self.name}' ({self.state_id}) given new " - f'outcome index={MirrorState._last_state_outcome}') - - MirrorState._last_state_id = None # Flag that the message was handled - if MirrorState._last_state_outcome is not None: - Logger.localwarn(f" StateMachine '{self.name}' ({self.state_id}) processing " - f'outcome index={MirrorState._last_state_outcome}') - outcome = self.on_exit_mirror(userdata, MirrorState._last_state_outcome) - MirrorState._last_state_outcome = None # Flag that the message was handled - return outcome - # Process the current state outcome = self._current_state.execute_mirror(userdata) if outcome is not None: @@ -208,8 +191,12 @@ 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._target_path) # 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: @@ -230,6 +217,8 @@ 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}) : " f'Already processed outcome={self._last_outcome} for ' From 04f49cfecfc7ae488af69eafbe4916c324b584e5 Mon Sep 17 00:00:00 2001 From: David Conner Date: Fri, 16 Aug 2024 01:08:09 -0400 Subject: [PATCH 08/23] update create_repo script --- README.md | 26 +++++++++-- flexbe_widget/bin/create_repo | 81 +++++++++++++++++++++-------------- 2 files changed, 71 insertions(+), 36 deletions(-) diff --git a/README.md b/README.md index a82dd9f..fb7ad55 100644 --- a/README.md +++ b/README.md @@ -34,15 +34,33 @@ Next, navigate to the "ros2_ws" top-level directory and build FlexBE: colcon build -Furthermore, create your own repository for behavior development (contains examples): - ros2 run flexbe_widget create_repo [your_project_name] +## 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. + +For example, running + `ros2 run flexbe_widget create_repo my_project my_flexbe_project` + +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` + + +These are intended to contain your custom FlexBE state implementations and HFSM-based behaviors. This version of the flexbe_behavior_engine requires version 4.0+ of the FlexBE user interface. It is recommended to install the FlexBE user interface by following one of these steps: + * https://github.com/FlexBE/flexbe_webui.git - new Python-based webserver version (preferred) * 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 ## Usage @@ -98,4 +116,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_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" From 2c35eec0446a2dcbfdf73eae039f818b54d66efc Mon Sep 17 00:00:00 2001 From: David Conner Date: Mon, 19 Aug 2024 20:44:06 -0400 Subject: [PATCH 09/23] clarify state map message --- flexbe_core/flexbe_core/core/state_map.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flexbe_core/flexbe_core/core/state_map.py b/flexbe_core/flexbe_core/core/state_map.py index 130a635..8906557 100644 --- a/flexbe_core/flexbe_core/core/state_map.py +++ b/flexbe_core/flexbe_core/core/state_map.py @@ -47,7 +47,7 @@ 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.""" From 82f9aa89b931e3844551e3a0ca95edaf8b6a0b05 Mon Sep 17 00:00:00 2001 From: David Conner Date: Mon, 19 Aug 2024 20:45:01 -0400 Subject: [PATCH 10/23] fix issue with input action server --- .../flexbe_input/input_action_server.py | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/flexbe_input/flexbe_input/input_action_server.py b/flexbe_input/flexbe_input/input_action_server.py index 9087f69..4853307 100644 --- a/flexbe_input/flexbe_input/input_action_server.py +++ b/flexbe_input/flexbe_input/input_action_server.py @@ -172,19 +172,23 @@ def execute_callback(self, goal_handle): return result else: 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 - result.data = str(pickle.dumps(input_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(result.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 + 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)) if data_len != expected_elements: result.data = (f'Invalid number of elements {data_len} not {expected_elements} ' From a9f04230d20ff55cb09dfd8664092ad73181eda1 Mon Sep 17 00:00:00 2001 From: David Conner Date: Mon, 19 Aug 2024 20:46:00 -0400 Subject: [PATCH 11/23] reduce default wait durations on proxy start ups; reduce start up spam --- .../flexbe_core/proxy/proxy_action_client.py | 8 +++---- .../flexbe_core/proxy/proxy_service_caller.py | 8 +++---- .../proxy/proxy_subscriber_cached.py | 22 +++++++++---------- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/flexbe_core/flexbe_core/proxy/proxy_action_client.py b/flexbe_core/flexbe_core/proxy/proxy_action_client.py index d7e8989..a481a9a 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_action_client.py +++ b/flexbe_core/flexbe_core/proxy/proxy_action_client.py @@ -88,9 +88,9 @@ 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. @@ -104,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): 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 6e4b28c..7806cc6 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: @@ -147,10 +147,10 @@ def subscribe(self, topic, msg_type, callback=None, buffered=False, qos=None, in # ' - 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: From a25a1184a1c83410eb9b13fbaf095c737df9f9e7 Mon Sep 17 00:00:00 2001 From: David Conner Date: Sat, 24 Aug 2024 22:42:49 -0400 Subject: [PATCH 12/23] 3.0.7 --- flexbe_behavior_engine/CHANGELOG.rst | 4 +++- flexbe_behavior_engine/package.xml | 2 +- flexbe_core/CHANGELOG.rst | 9 +++++++++ .../flexbe_core/core/operatable_state_machine.py | 2 +- flexbe_core/flexbe_core/core/state_machine.py | 1 + flexbe_core/flexbe_core/core/state_map.py | 3 ++- .../flexbe_core/proxy/proxy_action_client.py | 14 +++++++------- flexbe_core/package.xml | 2 +- flexbe_core/setup.py | 2 +- flexbe_input/CHANGELOG.rst | 6 ++++++ flexbe_input/flexbe_input/input_action_server.py | 2 +- flexbe_input/flexbe_input/input_gui.py | 14 +++++++------- flexbe_input/package.xml | 2 +- flexbe_input/setup.py | 2 +- flexbe_mirror/CHANGELOG.rst | 6 ++++++ flexbe_mirror/package.xml | 2 +- flexbe_mirror/setup.py | 2 +- flexbe_msgs/CHANGELOG.rst | 4 ++++ flexbe_msgs/package.xml | 2 +- flexbe_onboard/CHANGELOG.rst | 4 ++++ flexbe_onboard/package.xml | 2 +- flexbe_onboard/setup.py | 2 +- flexbe_states/CHANGELOG.rst | 5 +++++ flexbe_states/package.xml | 2 +- flexbe_states/setup.py | 2 +- flexbe_testing/CHANGELOG.rst | 4 ++++ flexbe_testing/package.xml | 2 +- flexbe_widget/CHANGELOG.rst | 4 ++++ flexbe_widget/package.xml | 2 +- 29 files changed, 78 insertions(+), 32 deletions(-) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 4724867..b5e4e37 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,9 +2,11 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -3.0.6 (2024-08-05) +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..e78adb8 100644 --- a/flexbe_behavior_engine/package.xml +++ b/flexbe_behavior_engine/package.xml @@ -1,7 +1,7 @@ flexbe_behavior_engine - 3.0.6 + 3.0.7 A meta-package to aggregate all the FlexBE packages diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index c7f1003..ac3ea9a 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/flexbe_core/flexbe_core/core/operatable_state_machine.py b/flexbe_core/flexbe_core/core/operatable_state_machine.py index 3a37d38..de3b960 100644 --- a/flexbe_core/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/flexbe_core/core/operatable_state_machine.py @@ -195,7 +195,7 @@ def _execute_current_state(self): 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}") + f"handling the transition cmd msg='{command_msg}'") self._force_transition = True outcome = self.outcomes[command_msg.outcome] diff --git a/flexbe_core/flexbe_core/core/state_machine.py b/flexbe_core/flexbe_core/core/state_machine.py index 013eda3..1a52e16 100644 --- a/flexbe_core/flexbe_core/core/state_machine.py +++ b/flexbe_core/flexbe_core/core/state_machine.py @@ -143,6 +143,7 @@ def execute(self, userdata): return outcome def on_enter(self, userdata): + """Call on entering the state machine.""" self.assert_consistent_transitions() self._entering = False self._current_state = self.initial_state diff --git a/flexbe_core/flexbe_core/core/state_map.py b/flexbe_core/flexbe_core/core/state_map.py index 8906557..436fe2f 100644 --- a/flexbe_core/flexbe_core/core/state_map.py +++ b/flexbe_core/flexbe_core/core/state_map.py @@ -47,7 +47,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' (resolved {self._num_collision_processed} state map id 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.""" diff --git a/flexbe_core/flexbe_core/proxy/proxy_action_client.py b/flexbe_core/flexbe_core/proxy/proxy_action_client.py index a481a9a..5ff19f7 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_action_client.py +++ b/flexbe_core/flexbe_core/proxy/proxy_action_client.py @@ -53,13 +53,13 @@ class ProxyActionClient: _client_sync_lock = Lock() _goal_status_dict = { - 0: "UNKNOWN", - 1: "ACCEPTED", - 2: "EXECUTING", - 3: "CANCELING", - 4: "SUCCEEDED", - 5: "CANCELED", - 6: "ABORTED" + 0: 'UNKNOWN', + 1: 'ACCEPTED', + 2: 'EXECUTING', + 3: 'CANCELING', + 4: 'SUCCEEDED', + 5: 'CANCELED', + 6: 'ABORTED' } @staticmethod diff --git a/flexbe_core/package.xml b/flexbe_core/package.xml index dbc72c4..7b8d29b 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 + 3.0.7 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..e113df8 100644 --- a/flexbe_core/setup.py +++ b/flexbe_core/setup.py @@ -6,7 +6,7 @@ setup( name=PACKAGE_NAME, - version='3.0.6', + version='3.0.7', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]), diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index 7abb614..7041ee9 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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/input_action_server.py b/flexbe_input/flexbe_input/input_action_server.py index 4853307..aa3a51a 100644 --- a/flexbe_input/flexbe_input/input_action_server.py +++ b/flexbe_input/flexbe_input/input_action_server.py @@ -178,7 +178,7 @@ def execute_callback(self, goal_handle): 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) + 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): diff --git a/flexbe_input/flexbe_input/input_gui.py b/flexbe_input/flexbe_input/input_gui.py index e3b17b9..5a5650d 100644 --- a/flexbe_input/flexbe_input/input_gui.py +++ b/flexbe_input/flexbe_input/input_gui.py @@ -61,7 +61,7 @@ def __init__(self, prompt): self.input_line = None def clear_layout(self): - # Clear prior layout + """Clear prior layout of widget.""" while self.main_layout.count(): item = self.main_layout.takeAt(0) widget = item.widget() @@ -70,7 +70,7 @@ def clear_layout(self): widget.deleteLater() def set_layout(self, prompt, items=None): - # Update the layout + """Update the widget layout.""" self.clear_layout() self.prompt = QLabel(self) @@ -179,15 +179,15 @@ def set_layout(self, prompt, items=None): def change_selection(self): """Print selection.""" if self.combo_box is None: - print("Unknown combo box - why is this called!", flush=True) + 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" + 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() @@ -195,8 +195,8 @@ def set_selection(self): def set_input(self): """Set input text from GUI.""" if self.input_line is None: - print("Unknown combo box - why is this called!", flush=True) - self.input = "unknown" + print('Unknown combo box - why is this called!', flush=True) + self.input = 'unknown' else: self.input = self.input_line.text() diff --git a/flexbe_input/package.xml b/flexbe_input/package.xml index 91a882b..3e933ae 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 + 3.0.7 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..3c81c04 100644 --- a/flexbe_input/setup.py +++ b/flexbe_input/setup.py @@ -6,7 +6,7 @@ setup( name=PACKAGE_NAME, - version='3.0.6', + version='3.0.7', 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..0d8cc30 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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/package.xml b/flexbe_mirror/package.xml index e6ddc26..b6ddd51 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 + 3.0.7 flexbe_mirror implements functionality to remotely mirror an executed behavior. diff --git a/flexbe_mirror/setup.py b/flexbe_mirror/setup.py index e2473e7..bb6e6b9 100644 --- a/flexbe_mirror/setup.py +++ b/flexbe_mirror/setup.py @@ -7,7 +7,7 @@ setup( name=PACKAGE_NAME, - version='3.0.6', + version='3.0.7', 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..8467754 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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/package.xml b/flexbe_msgs/package.xml index 3d02a40..efca18f 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 + 3.0.7 flexbe_msgs provides the messages used by FlexBE. diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index dc60854..b0491e1 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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/package.xml b/flexbe_onboard/package.xml index 8068892..979dc30 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 + 3.0.7 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..375fd68 100644 --- a/flexbe_onboard/setup.py +++ b/flexbe_onboard/setup.py @@ -9,7 +9,7 @@ setup( name=package_name, - version='3.0.6', + version='3.0.7', packages=find_packages(), data_files=[ (os.path.join('share', package_name), glob('launch/*.launch.py')), diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index e94bf5c..98fe400 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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/package.xml b/flexbe_states/package.xml index 09970b8..2087554 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 + 3.0.7 flexbe_states provides a collection of common generic predefined states. diff --git a/flexbe_states/setup.py b/flexbe_states/setup.py index a7a927a..f4699fb 100644 --- a/flexbe_states/setup.py +++ b/flexbe_states/setup.py @@ -9,7 +9,7 @@ setup( name=PACKAGE_NAME, - version='3.0.6', + version='3.0.7', 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..5224c30 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.7 (2024-08-24) +------------------ +* add initialize_flexbe_core for common initialization + 3.0.6 (2024-08-05) ------------------ diff --git a/flexbe_testing/package.xml b/flexbe_testing/package.xml index 918c2bc..6ea9db6 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 + 3.0.7 flexbe_testing provides a framework for unit testing states. diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index c4c9d2d..388c2d5 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.7 (2024-08-24) +------------------ +* update create_repo script + 3.0.6 (2024-08-05) ------------------ diff --git a/flexbe_widget/package.xml b/flexbe_widget/package.xml index b8c5e0d..f1f0d88 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 + 3.0.7 flexbe_widget implements some smaller scripts for the behavior engine. From 9580cabc5a2e8dd1a1d050a004eb9c2d2f094106 Mon Sep 17 00:00:00 2001 From: David Conner Date: Sat, 24 Aug 2024 23:07:50 -0400 Subject: [PATCH 13/23] 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; this breaks API with flexbe_app and requires version 4.1.0+ of the FlexBE WebUI API --- README.md | 55 +++-- flexbe_core/flexbe_core/__init__.py | 2 +- flexbe_core/flexbe_core/behavior.py | 35 ++- flexbe_core/flexbe_core/core/__init__.py | 32 ++- .../flexbe_core/core/concurrency_container.py | 58 +++-- flexbe_core/flexbe_core/core/event_state.py | 14 +- .../flexbe_core/core/lockable_state.py | 25 +- .../core/lockable_state_machine.py | 12 +- .../core/manually_transitionable_state.py | 6 +- .../flexbe_core/core/operatable_state.py | 21 +- .../core/operatable_state_machine.py | 110 +++++---- .../flexbe_core/core/preemptable_state.py | 3 +- .../core/preemptable_state_machine.py | 9 +- flexbe_core/flexbe_core/core/state.py | 3 +- flexbe_core/flexbe_core/core/state_machine.py | 4 +- flexbe_core/flexbe_core/core/state_map.py | 23 +- flexbe_core/flexbe_core/core/topics.py | 15 +- flexbe_core/test/test_core.py | 232 +++++++++++------- flexbe_core/test/test_exceptions.py | 34 ++- flexbe_core/test/test_exceptions_spin.py | 38 ++- flexbe_core/test/test_logger.py | 24 +- flexbe_core/test/test_proxies.py | 46 ++-- flexbe_mirror/flexbe_mirror/flexbe_mirror.py | 44 ++-- .../mirror_concurrency_container.py | 19 +- flexbe_mirror/flexbe_mirror/mirror_state.py | 28 ++- .../flexbe_mirror/mirror_state_machine.py | 37 +-- flexbe_msgs/CMakeLists.txt | 1 + flexbe_msgs/action/BehaviorExecution.action | 4 +- flexbe_msgs/msg/OutcomeRequest.msg | 2 +- flexbe_msgs/msg/StateMapMsg.msg | 8 + .../flexbe_onboard/flexbe_onboard.py | 13 +- flexbe_widget/CMakeLists.txt | 1 + flexbe_widget/bin/flexbe_outcome_listener | 6 + .../flexbe_widget/behavior_action_server.py | 6 +- .../flexbe_widget/behavior_launcher.py | 36 ++- .../flexbe_widget/flexbe_outcome_listener.py | 103 ++++++++ 36 files changed, 726 insertions(+), 383 deletions(-) create mode 100644 flexbe_msgs/msg/StateMapMsg.msg create mode 100644 flexbe_widget/bin/flexbe_outcome_listener create mode 100644 flexbe_widget/flexbe_widget/flexbe_outcome_listener.py diff --git a/README.md b/README.md index fb7ad55..eb03aea 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. @@ -23,64 +23,73 @@ Rolling ![ROS Build Farm](https://build.ros2.org/job/Rdev__flexbe_behavior_engin ## 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` -Next, navigate to the "ros2_ws" top-level directory and build FlexBE: + `git clone https://github.com/FlexBE/flexbe_behavior_engine.git` - colcon build +Next, navigate to the "ros2_ws" top-level directory and build FlexBE: + `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>` + `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. For example, running + `ros2 run flexbe_widget create_repo my_project my_flexbe_project` 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` - + - `${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` These are intended to contain your custom FlexBE state implementations and HFSM-based behaviors. -This version of the flexbe_behavior_engine requires version 4.0+ of the FlexBE user interface. +This release of the FlexBE Behavior Engine requires version 4.1+ of the FlexBE UI. +This breaks compatability with the older FlexBE App and now requires use of the FlexBE WebUI tool. + +It is recommended to install the FlexBE WebUI user interface: -It is recommended to install the FlexBE user interface by following one of these steps: - * https://github.com/FlexBE/flexbe_webui.git - new Python-based webserver version (preferred) - * https://github.com/FlexBE/flexbe_app.git - classic FlexBE App (iron or ros2-devel branches) + [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` - > Note: replace `flexbe_webui` with `flexbe_app` to run the "classic" UI (after `ros2 run flexbe_app nwjs_install`). +During testing is is recommended to start the base nodes and the UI client separately: -Use the following launch file to run both of the above, e.g., for testing on a single computer: + `ros2 launch flexbe_webui flexbe_ocs.launch.py headless:=True` - ros2 launch flexbe_webui flexbe_full.launch.py + `ros2 run flexbe_webui webui_client` + + 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 diff --git a/flexbe_core/flexbe_core/__init__.py b/flexbe_core/flexbe_core/__init__.py index d1d92f2..0f07353 100644 --- a/flexbe_core/flexbe_core/__init__.py +++ b/flexbe_core/flexbe_core/__init__.py @@ -43,7 +43,7 @@ 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 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/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 0d8d501..04fbe94 100644 --- a/flexbe_core/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/flexbe_core/core/concurrency_container.py @@ -38,6 +38,7 @@ from flexbe_core.core.lockable_state_machine import LockableStateMachine from flexbe_core.core.operatable_state_machine import OperatableStateMachine 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 @@ -95,10 +96,6 @@ def get_required_autonomy(self, outcome, state): def _execute_current_state(self): """Execute the current states within this concurrency container.""" # execute all states that are done with sleeping and determine next sleep duration - if self._entering: - Logger.localerr(f"CC: Why is entering flag set here '{self.name}' of '{self.path}'?") - self.on_enter(self._userdata) - self._inner_sync_request = False # clear prior request for lower level state self._current_state = [] # Concurrency container has multiple active states so use list @@ -107,18 +104,18 @@ def _execute_current_state(self): # 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"ConcurrencyContainer '{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 = {} @@ -132,7 +129,7 @@ def _execute_current_state(self): 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.localinfo(f"CC '{self.path}' is waiting on user to confirm outcome") + Logger.loginfo_throttle(2.0, f"CC '{self.path}' is waiting on user to confirm outcome") return None for state in self._states: @@ -141,13 +138,13 @@ def _execute_current_state(self): 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"ConcurrencyContainer '{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 @@ -167,7 +164,7 @@ 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 ConcurrencyContainer '{self.name}'") continue @@ -204,9 +201,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 @@ -227,7 +224,7 @@ 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)) + 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, @@ -236,8 +233,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 - Logger.localinfo(f"controlled CC '{self.name}' from '{self.path}'permitting outcome '{outcome}' ") - self._publish_outcome(outcome) self._force_transition = False self._last_outcome = outcome @@ -251,22 +246,27 @@ 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 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('%', '%%')) - return result def on_enter(self, userdata): # pylint: disable=W0613 """Call on entering the concurrency container.""" - Logger.localinfo(f" CC on_enter for '{self.path}' ... ") super().on_enter(userdata) self._returned_outcomes = {} for state in self._states: @@ -279,17 +279,29 @@ def on_exit(self, userdata, states=None): 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 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.path)) + # 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. diff --git a/flexbe_core/flexbe_core/core/event_state.py b/flexbe_core/flexbe_core/core/event_state.py index be838b4..ed3c69e 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 immedately 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..0fe2c72 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,42 @@ 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 c724d6e..3b3970a 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 @@ -66,7 +67,7 @@ def _operatable_execute(self, *args, **kwargs): if self._is_controlled: # 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,7 +75,7 @@ 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)) + 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, @@ -84,17 +85,25 @@ 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: - Logger.localinfo(f"controlled State '{self.name}' from '{self.path}'permitting outcome '{outcome}' ") - self._publish_outcome(outcome) + Logger.localinfo(f"controlled State '{self.name}' from '{self.path}'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 de3b960..0c28df2 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,18 +180,12 @@ def get_latest_status(self): # execution def _execute_current_state(self): - - if self._entering: - # On entering this state machine - Logger.localerr(f"OSM: Why is entering flag set here '{self.name}' of '{self.path}'?") - raise Exception("entering flag still set L187 of OSM - '{self.path}'") - 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.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"Statemachine '{self.name}' from '{self.path}' is " @@ -202,29 +196,31 @@ def _execute_current_state(self): self._manual_transition_requested = outcome self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='transition', - args=[command_msg.target, self.name])) + args=[f'{command_msg.target}', f'{self.state_id}'])) Logger.localwarn(f"--> Manually triggered outcome {outcome} of statemachine '{self.name}'") self._last_outcome = outcome - self._publish_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.localinfo(f"OSM '{self.path}' is waiting on user to confirm outcome") + Logger.loginfo_throttle(2.0, f"OSM '{self.path}' is waiting on user to confirm outcome") return None try: outcome = super()._execute_current_state() self._last_exception = None except Exception as exc: # pylint: disable=W0703 - # catch any exception and log here, but re-raise to stop behavior - 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! - raise exc + 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: # request outcome because autonomy level is too low @@ -235,7 +231,7 @@ 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)) + 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, @@ -243,9 +239,7 @@ def _execute_current_state(self): self._last_requested_outcome = outcome outcome = None - if outcome is not None and outcome in self.outcomes: - Logger.localinfo(f"controlled SM '{self.name}' from '{self.path}'returning outcome '{outcome}' ") - self._publish_outcome(outcome) + if outcome is not None: self._force_transition = False self._last_outcome = outcome @@ -254,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: @@ -296,7 +291,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}') @@ -325,7 +320,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. @@ -337,11 +332,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} ...') @@ -446,26 +444,48 @@ def _notify_stop(self): def on_enter(self, userdata=None): # pylint: disable=W0613 """Call on entering the operatable state machine.""" - Logger.localinfo(f"OSM on enter for '{self.name}' from '{self.path}' ...") + 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.""" - Logger.localinfo(f"SM on exit for '{self.name}' from '{self.path}' ...") - 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.path)) + # 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 86f4318..684d942 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,12 +74,12 @@ 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, rclpy_context=None): """Spin the execute loop for preemptable portion.""" @@ -105,7 +104,7 @@ def spin(self, userdata=None, rclpy_context=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 diff --git a/flexbe_core/flexbe_core/core/state.py b/flexbe_core/flexbe_core/core/state.py index a655dec..080a9c6 100644 --- a/flexbe_core/flexbe_core/core/state.py +++ b/flexbe_core/flexbe_core/core/state.py @@ -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 1a52e16..230f4cc 100644 --- a/flexbe_core/flexbe_core/core/state_machine.py +++ b/flexbe_core/flexbe_core/core/state_machine.py @@ -139,6 +139,7 @@ def execute(self, userdata): if outcome: # Exit this statemachine self.on_exit(self._userdata) + self._publish_outcome(outcome) return outcome @@ -146,12 +147,13 @@ 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}'") + 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.""" diff --git a/flexbe_core/flexbe_core/core/state_map.py b/flexbe_core/flexbe_core/core/state_map.py index 436fe2f..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 = {} @@ -57,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): """ @@ -87,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.""" @@ -135,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/test/test_core.py b/flexbe_core/test/test_core.py index f8ad0a6..7ad2fe7 100755 --- a/flexbe_core/test/test_core.py +++ b/flexbe_core/test/test_core.py @@ -34,9 +34,9 @@ import unittest from flexbe_core import ConcurrencyContainer, EventState, OperatableStateMachine, initialize_flexbe_core -from flexbe_core.core import PreemptableState -from flexbe_core.core.exceptions import StateMachineError -from flexbe_core.core.topics import Topics +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 @@ -44,7 +44,7 @@ import rclpy from rclpy.executors import MultiThreadedExecutor -from std_msgs.msg import Bool, Empty, String, UInt32 +from std_msgs.msg import Bool, Empty, Int32, String, 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 + __TIME_SLEEP = 0.025 # Sleep time for loops + __EXECUTE_TIMEOUT_SEC = 0.025 # Timeout in executor loops for spin once + __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) @@ -134,29 +139,29 @@ def setUp(self): 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.""" @@ -165,6 +170,7 @@ def _create(self): 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'}, @@ -172,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))) @@ -214,26 +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) - 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) @@ -284,7 +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) + 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 @@ -293,53 +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) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) state, sm = self._create() fb_topic = Topics._CMD_FEEDBACK_TOPIC @@ -349,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 ...') @@ -370,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')) @@ -381,7 +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) + 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)) @@ -389,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 ... ') @@ -454,20 +497,22 @@ 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) + 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 no message is expected @@ -478,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 @@ -486,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 @@ -494,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') @@ -512,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! ') @@ -534,7 +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) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestCore.__EXECUTE_TIMEOUT_SEC) cc = ConcurrencyContainer(outcomes=['done', 'error'], conditions=[ ('error', [('main', 'error')]), @@ -555,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) @@ -668,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.""" @@ -677,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 @@ -689,26 +736,35 @@ def execute(self, userdata): 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 63432b9..e67417a 100755 --- a/flexbe_core/test/test_exceptions.py +++ b/flexbe_core/test/test_exceptions.py @@ -45,6 +45,8 @@ class TestExceptions(unittest.TestCase): """Test FlexBE Exception handling.""" test = 0 + __EXECUTE_TIMEOUT_SEC=0.025 + __TIME_SLEEP = 0.05 # Sleep time for loops def __init__(self, *args, **kwargs): """Initialize TestExceptions instance.""" @@ -69,24 +71,24 @@ 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) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class ReturnInvalidOutcomeState(EventState): """Local Test state definition.""" @@ -101,7 +103,9 @@ def execute(self, userdata): try: sm = OperatableStateMachine(outcomes=['done']) with sm: - OperatableStateMachine.add('state', ReturnInvalidOutcomeState(), transitions={'done': 'done'}) + 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}'") @@ -114,7 +118,7 @@ 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) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class ReturnDoneState(EventState): """Local Test state definition.""" @@ -127,7 +131,9 @@ 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'}) @@ -146,7 +152,7 @@ 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) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class AccessInvalidInputState(EventState): """Local Test state definition.""" @@ -160,7 +166,9 @@ 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: @@ -176,7 +184,7 @@ 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) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class SetInvalidOutputState(EventState): """Local Test state definition.""" @@ -206,7 +214,7 @@ 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) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class AccessValidInputState(EventState): """Local Test state definition.""" @@ -236,7 +244,7 @@ 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) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestExceptions.__EXECUTE_TIMEOUT_SEC) class ModifyInputKeyState(EventState): """Local Test state definition.""" diff --git a/flexbe_core/test/test_exceptions_spin.py b/flexbe_core/test/test_exceptions_spin.py index adde1f5..cc087f3 100644 --- a/flexbe_core/test/test_exceptions_spin.py +++ b/flexbe_core/test/test_exceptions_spin.py @@ -45,6 +45,8 @@ class TestExceptionsSpin(unittest.TestCase): """Test FlexBE Exception handling.""" test = 0 + __EXECUTE_TIMEOUT_SEC=0.025 + __TIME_SLEEP = 0.05 # Sleep time for loops def __init__(self, *args, **kwargs): """Initialize TestExceptionsSpin instance.""" @@ -67,34 +69,35 @@ def setUp(self): 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=0.1) + 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(0.1) + time.sleep(TestExceptionsSpin.__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(TestExceptionsSpin.__TIME_SLEEP) self.executor.shutdown() - time.sleep(0.1) + 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(0.2) + 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=1) + 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 ...') @@ -105,6 +108,8 @@ def on_enter(self, userdata): 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) @@ -117,21 +122,24 @@ 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) + 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'}) @@ -145,19 +153,21 @@ 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) + 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'}) @@ -171,19 +181,21 @@ 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) + 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'}) @@ -197,19 +209,21 @@ 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) + 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'}) @@ -223,19 +237,21 @@ 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) + 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'}) diff --git a/flexbe_core/test/test_logger.py b/flexbe_core/test/test_logger.py index 49a93f5..a3b14e5 100644 --- a/flexbe_core/test/test_logger.py +++ b/flexbe_core/test/test_logger.py @@ -45,6 +45,8 @@ class TestLogger(unittest.TestCase): """Test FlexBE Logger handling.""" test = 0 + __EXECUTE_TIMEOUT_SEC=0.025 + __TIME_SLEEP = 0.05 # Sleep time for loops def __init__(self, *args, **kwargs): """Initialize TestLogger instance.""" @@ -64,22 +66,22 @@ def setUp(self): 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.""" @@ -88,7 +90,7 @@ def test_throttle_logger_one(self): self.node.declare_parameter('throttle_logging_clear_ratio', 0.25) initialize_flexbe_core(self.node) # Update the logger node - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestLogger.__EXECUTE_TIMEOUT_SEC) class ThrottleSingleLog(EventState): """Local Test state definition.""" @@ -128,7 +130,7 @@ def test_throttle_logger_err_multi(self): self.node.declare_parameter('throttle_logging_clear_ratio', 0.35) initialize_flexbe_core(self.node) # Update the logger node - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestLogger.__EXECUTE_TIMEOUT_SEC) class ThrottleMultiLog(EventState): """Local Test state definition.""" @@ -169,7 +171,7 @@ def test_throttle_logger_multiple_params(self): initialize_flexbe_core(self.node) # Update the logger node - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestLogger.__EXECUTE_TIMEOUT_SEC) class ThrottleMultiLog(EventState): """Local Test state definition.""" @@ -200,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) @@ -214,7 +216,7 @@ def test_throttle_logger_multiple(self): self.node.declare_parameter('throttle_logging_clear_ratio', 0.22) initialize_flexbe_core(self.node) # Update the logger node - rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=TestLogger.__EXECUTE_TIMEOUT_SEC) class ThrottleMultiLog(EventState): """Local Test state definition.""" @@ -245,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*.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 49f801a..560e337 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.025 + __TIME_SLEEP = 0.05 # 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} ') diff --git a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py index c6a5881..fdce862 100644 --- a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py @@ -44,11 +44,11 @@ def set_thread_name(name): 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 @@ -123,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, 2) + # 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 @@ -143,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): @@ -155,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): @@ -417,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) @@ -753,7 +755,7 @@ 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, self._state_map) Logger.localinfo(f"Mirror finished spin with result '{result}' after {self.get_elapsed_str(start_time)} s") @@ -767,7 +769,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 +790,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 +804,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 +822,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 +856,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 +870,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 b47201f..505ecad 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).""" @@ -70,7 +71,11 @@ def on_exit_mirror(self, userdata, desired_outcome=-1, states=None): self._current_state = None self._returned_outcomes = {} if desired_outcome != -1: - self._last_outcome = self.outcomes[desired_outcome] + 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): @@ -86,7 +91,7 @@ def execute_mirror(self, userdata): # Handle outcome of this internal SM if self._last_outcome is not None: Logger.localwarn(f"Mirror SM concurrency execute for '{self.name.replace('_mirror', '')}' of " - f" '{self.path.replace('_mirror', '')}' ({self._state_id}) : " + 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') @@ -118,7 +123,7 @@ def _execute_current_state_mirror(self, userdata): 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._target_path) # Notify back at top-level before exit + MirrorState.publish_update(self.state_id) # Notify back at top-level before exit return None def get_deep_states(self): @@ -139,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)}'") return deep_states diff --git a/flexbe_mirror/flexbe_mirror/mirror_state.py b/flexbe_mirror/flexbe_mirror/mirror_state.py index d6b7d9d..7558654 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 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,13 +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: if desired_outcome != -1: - self._last_outcome = self.outcomes[desired_outcome] + 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 6d7f2dc..a28d699 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 @@ -77,11 +78,11 @@ def spin(self, start_time, state_map): 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 @@ -112,7 +113,7 @@ def spin(self, start_time, state_map): 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}') @@ -129,7 +130,7 @@ def spin(self, start_time, state_map): 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}') @@ -141,7 +142,7 @@ def spin(self, start_time, state_map): self._last_deep_states_list = deep_states 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: @@ -191,8 +192,8 @@ 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: - Logger.localinfo(f"SM {self.name.replace('_mirror', '')} is done, but wait for outcome message.") - MirrorState.publish_update(self._target_path) # Notify back at sm-level before exit + # 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 " @@ -201,10 +202,10 @@ def _execute_current_state_mirror(self, userdata): 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 @@ -220,7 +221,7 @@ def execute_mirror(self, userdata): # 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') @@ -276,7 +277,7 @@ 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.""" @@ -285,9 +286,13 @@ def on_exit_mirror(self, userdata, desired_outcome=-1): self._current_state._entering = True self._current_state.on_exit_mirror(userdata, -1) # Preempted if desired_outcome != -1: - self._last_outcome = self.outcomes[desired_outcome] + 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_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/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_onboard/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py index fac98c1..d6172f1 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 @@ -93,6 +93,7 @@ def __init__(self): # only at onboard level self._heartbeat_pub = self.create_publisher(BehaviorSync, Topics._ONBOARD_HEARTBEAT_TOPIC, 10) self._status_pub = self.create_publisher(BEStatus, Topics._ONBOARD_STATUS_TOPIC, 10) + self._state_map_pub = self.create_publisher(StateMapMsg, Topics._STATE_MAP_TOPIC, 2) latching_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) self._version_sub = self.create_subscription(String, Topics._UI_VERSION_TOPIC, @@ -315,7 +316,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_widget/CMakeLists.txt b/flexbe_widget/CMakeLists.txt index 5abb5d1..f216901 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_outcome_listener DESTINATION lib/${PROJECT_NAME} ) diff --git a/flexbe_widget/bin/flexbe_outcome_listener b/flexbe_widget/bin/flexbe_outcome_listener new file mode 100644 index 0000000..34e9ec2 --- /dev/null +++ b/flexbe_widget/bin/flexbe_outcome_listener @@ -0,0 +1,6 @@ +#!/usr/bin/env python3 + +from flexbe_widget.flexbe_outcome_listener import flexbe_outcome_listener_main + +if __name__ == '__main__': + flexbe_outcome_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..082e88b 100644 --- a/flexbe_widget/flexbe_widget/behavior_launcher.py +++ b/flexbe_widget/flexbe_widget/behavior_launcher.py @@ -46,6 +46,7 @@ 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 @@ -77,6 +78,7 @@ def __init__(self): self._status_pub = self.create_publisher(BEStatus, Topics._ONBOARD_STATUS_TOPIC, 100) self._mirror_pub = self.create_publisher(ContainerStructure, Topics._MIRROR_STRUCTURE_TOPIC, 100) self._heartbeat_pub = self.create_publisher(Int32, Topics._LAUNCHER_HEARTBEAT_TOPIC, 2) + self._state_map_pub = self.create_publisher(StateMapMsg, Topics._STATE_MAP_OCS_TOPIC, 2) self._behavior_lib = BehaviorLibrary(self) @@ -120,10 +122,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 +164,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 +213,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_outcome_listener.py b/flexbe_widget/flexbe_widget/flexbe_outcome_listener.py new file mode 100644 index 0000000..a9d3190 --- /dev/null +++ b/flexbe_widget/flexbe_widget/flexbe_outcome_listener.py @@ -0,0 +1,103 @@ +#!/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. + +import rclpy +from rclpy.node import Node + +from flexbe_core.core import StateMap, Topics + +class FlexbeOutcomeListener(Node): + """Simple lister to echo FlexBE state machine information.""" + def __init__(self): + super().__init__('flexbe_outcome_listener') + + + # Suppress the node name and severity information (ChatGPT led me astray) + # import logging + # formatter = logging.Formatter('[%(asctime)s] %(message)s') + # console_handler = logging.StreamHandler() + # console_handler.setFormatter(formatter) + + # self.get_logger().handlers.clear() + # self.get_logger().addHandler(console_handler) + + self._state_map = None + self._state_map_sub = self.create_subscription( + Topics._topic_types[Topics._OUTCOME_TOPIC], + Topics._OUTCOME_TOPIC, + self._outcome_callback, + 10) + self._state_map_sub # prevent unused variable warning + + self._state_map_sub = self.create_subscription( + Topics._topic_types[Topics._STATE_MAP_OCS_TOPIC], + Topics._STATE_MAP_OCS_TOPIC, + self._state_map_callback, + 10) + self._state_map_sub # prevent unused variable warning + + 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_outcome_listener_main(args=None): + + print('Suggest: export RCUTILS_CONSOLE_OUTPUT_FORMAT="[{time}] {message}" in terminal') + print(' Run this before starting behavior to get full state path informatio from state map.') + rclpy.init(args=args) + + outcomes = FlexbeOutcomeListener() + + 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_outcome_listener_main() \ No newline at end of file From 0a8d48b881f9e41f2a78ec21b0a4ab36431f8163 Mon Sep 17 00:00:00 2001 From: David Conner Date: Sun, 25 Aug 2024 00:29:07 -0400 Subject: [PATCH 14/23] 4.0.0 --- README.md | 5 +++-- flexbe_behavior_engine/CHANGELOG.rst | 5 +++++ flexbe_behavior_engine/package.xml | 2 +- flexbe_core/CHANGELOG.rst | 8 ++++++++ flexbe_core/package.xml | 2 +- flexbe_core/setup.py | 2 +- flexbe_input/CHANGELOG.rst | 5 +++++ flexbe_input/package.xml | 2 +- flexbe_input/setup.py | 2 +- flexbe_mirror/CHANGELOG.rst | 8 ++++++++ flexbe_mirror/package.xml | 2 +- flexbe_mirror/setup.py | 2 +- flexbe_msgs/CHANGELOG.rst | 8 ++++++++ flexbe_msgs/package.xml | 2 +- flexbe_onboard/CHANGELOG.rst | 8 ++++++++ flexbe_onboard/package.xml | 2 +- flexbe_onboard/setup.py | 2 +- flexbe_states/CHANGELOG.rst | 8 ++++++++ flexbe_states/package.xml | 2 +- flexbe_states/setup.py | 2 +- flexbe_testing/CHANGELOG.rst | 8 ++++++++ flexbe_testing/package.xml | 2 +- flexbe_widget/CHANGELOG.rst | 8 ++++++++ flexbe_widget/package.xml | 2 +- 24 files changed, 83 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index eb03aea..2fa9050 100644 --- a/README.md +++ b/README.md @@ -15,12 +15,13 @@ You may also want to check out the quick start tutorial demonstrations at [FlexB ![FlexBE CI](https://github.com/FlexBE/flexbe_behavior_engine/workflows/FlexBE%20CI/badge.svg?branch=ros2-devel) -Iron ![ROS Build Farm](https://build.ros2.org/job/Idev__flexbe_behavior_engine__ubuntu_jammy_amd64/badge/icon) - Jazzy ![ROS Build Farm](https://build.ros2.org/job/Jdev__flexbe_behavior_engine__ubuntu_noble_amd64/badge/icon) Rolling ![ROS Build Farm](https://build.ros2.org/job/Rdev__flexbe_behavior_engine__ubuntu_noble_amd64/badge/icon) +> Note: This version 4+ breaks compatability 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-*` diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index b5e4e37..09ac6b6 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------ diff --git a/flexbe_behavior_engine/package.xml b/flexbe_behavior_engine/package.xml index e78adb8..c9f8980 100644 --- a/flexbe_behavior_engine/package.xml +++ b/flexbe_behavior_engine/package.xml @@ -1,7 +1,7 @@ flexbe_behavior_engine - 3.0.7 + 4.0.0 A meta-package to aggregate all the FlexBE packages diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index ac3ea9a..ff05188 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/flexbe_core/package.xml b/flexbe_core/package.xml index 7b8d29b..16b706b 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.7 + 4.0.0 flexbe_core provides the core components for the FlexBE behavior engine. diff --git a/flexbe_core/setup.py b/flexbe_core/setup.py index e113df8..cd985fa 100644 --- a/flexbe_core/setup.py +++ b/flexbe_core/setup.py @@ -6,7 +6,7 @@ setup( name=PACKAGE_NAME, - version='3.0.7', + version='4.0.0', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]), diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index 7041ee9..660d09c 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/flexbe_input/package.xml b/flexbe_input/package.xml index 3e933ae..00654dc 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.7 + 4.0.0 flexbe_input enables to send data to onboard behavior when required. diff --git a/flexbe_input/setup.py b/flexbe_input/setup.py index 3c81c04..0736766 100644 --- a/flexbe_input/setup.py +++ b/flexbe_input/setup.py @@ -6,7 +6,7 @@ setup( name=PACKAGE_NAME, - version='3.0.7', + version='4.0.0', 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 0d8cc30..9f93459 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/flexbe_mirror/package.xml b/flexbe_mirror/package.xml index b6ddd51..addf993 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.7 + 4.0.0 flexbe_mirror implements functionality to remotely mirror an executed behavior. diff --git a/flexbe_mirror/setup.py b/flexbe_mirror/setup.py index bb6e6b9..c253853 100644 --- a/flexbe_mirror/setup.py +++ b/flexbe_mirror/setup.py @@ -7,7 +7,7 @@ setup( name=PACKAGE_NAME, - version='3.0.7', + version='4.0.0', 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 8467754..58a2991 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/flexbe_msgs/package.xml b/flexbe_msgs/package.xml index efca18f..eaf914b 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.7 + 4.0.0 flexbe_msgs provides the messages used by FlexBE. diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index b0491e1..3257d5f 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/flexbe_onboard/package.xml b/flexbe_onboard/package.xml index 979dc30..4203527 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.7 + 4.0.0 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 375fd68..b4dfd41 100644 --- a/flexbe_onboard/setup.py +++ b/flexbe_onboard/setup.py @@ -9,7 +9,7 @@ setup( name=package_name, - version='3.0.7', + version='4.0.0', packages=find_packages(), data_files=[ (os.path.join('share', package_name), glob('launch/*.launch.py')), diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index 98fe400..48f161c 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/flexbe_states/package.xml b/flexbe_states/package.xml index 2087554..a862fba 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.7 + 4.0.0 flexbe_states provides a collection of common generic predefined states. diff --git a/flexbe_states/setup.py b/flexbe_states/setup.py index f4699fb..7dce138 100644 --- a/flexbe_states/setup.py +++ b/flexbe_states/setup.py @@ -9,7 +9,7 @@ setup( name=PACKAGE_NAME, - version='3.0.7', + version='4.0.0', 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 5224c30..8136d81 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/flexbe_testing/package.xml b/flexbe_testing/package.xml index 6ea9db6..5d93ea4 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.7 + 4.0.0 flexbe_testing provides a framework for unit testing states. diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index 388c2d5..004d6ac 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 diff --git a/flexbe_widget/package.xml b/flexbe_widget/package.xml index f1f0d88..60cdf0a 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.7 + 4.0.0 flexbe_widget implements some smaller scripts for the behavior engine. From f176c0305f54643ac5123841dadae66ebbf4872e Mon Sep 17 00:00:00 2001 From: David Conner Date: Tue, 27 Aug 2024 00:06:45 -0400 Subject: [PATCH 15/23] use jazzy test; remove some stray spam; increase some loop timings in tests due to intermittant test failure --- .github/workflows/flexbe_ci.yml | 8 ++++++-- flexbe_core/flexbe_core/core/operatable_state.py | 6 +++--- flexbe_core/test/test_core.py | 4 ++-- flexbe_core/test/test_exceptions.py | 4 ++-- flexbe_core/test/test_exceptions_spin.py | 4 ++-- flexbe_core/test/test_logger.py | 4 ++-- flexbe_core/test/test_proxies.py | 4 ++-- flexbe_onboard/tests/test_onboard.py | 6 ++++-- 8 files changed, 23 insertions(+), 17 deletions(-) diff --git a/.github/workflows/flexbe_ci.yml b/.github/workflows/flexbe_ci.yml index d066c66..da63089 100644 --- a/.github/workflows/flexbe_ci.yml +++ b/.github/workflows/flexbe_ci.yml @@ -8,10 +8,14 @@ jobs: test: strategy: matrix: - ros: [rolling] + ros: [jazzy] #, rolling] include: + # - os: ubuntu-24.04 + # ros: rolling + # python: python3 + # ci_branch: ros2-devel - os: ubuntu-24.04 - ros: rolling + ros: jazzy python: python3 ci_branch: ros2-devel diff --git a/flexbe_core/flexbe_core/core/operatable_state.py b/flexbe_core/flexbe_core/core/operatable_state.py index 3b3970a..1fc9a01 100644 --- a/flexbe_core/flexbe_core/core/operatable_state.py +++ b/flexbe_core/flexbe_core/core/operatable_state.py @@ -85,7 +85,7 @@ 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: - Logger.localinfo(f"controlled State '{self.name}' from '{self.path}'permitting outcome '{outcome}' {self.__class__.__name__}") + # Logger.localinfo(f"controlled State '{self.name}' from '{self.path}'permitting outcome '{outcome}' {self.__class__.__name__}") self._force_transition = False return outcome @@ -101,8 +101,8 @@ def _publish_outcome(self, outcome): return outcome_index = self.outcomes.index(outcome) - Logger.localinfo('Publish outcome: State result: %s > %s (%d) (%d) (%s)' - % (self.name, outcome, outcome_index, self.state_id, self.__class__.__name__)) + # 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))) diff --git a/flexbe_core/test/test_core.py b/flexbe_core/test/test_core.py index 7ad2fe7..d73e3ab 100755 --- a/flexbe_core/test/test_core.py +++ b/flexbe_core/test/test_core.py @@ -117,8 +117,8 @@ class TestCore(unittest.TestCase): test = 0 - __TIME_SLEEP = 0.025 # Sleep time for loops - __EXECUTE_TIMEOUT_SEC = 0.025 # Timeout in executor loops for spin once + __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): diff --git a/flexbe_core/test/test_exceptions.py b/flexbe_core/test/test_exceptions.py index e67417a..8a072e9 100755 --- a/flexbe_core/test/test_exceptions.py +++ b/flexbe_core/test/test_exceptions.py @@ -45,8 +45,8 @@ class TestExceptions(unittest.TestCase): """Test FlexBE Exception handling.""" test = 0 - __EXECUTE_TIMEOUT_SEC=0.025 - __TIME_SLEEP = 0.05 # Sleep time for loops + __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.""" diff --git a/flexbe_core/test/test_exceptions_spin.py b/flexbe_core/test/test_exceptions_spin.py index cc087f3..fca7b03 100644 --- a/flexbe_core/test/test_exceptions_spin.py +++ b/flexbe_core/test/test_exceptions_spin.py @@ -45,8 +45,8 @@ class TestExceptionsSpin(unittest.TestCase): """Test FlexBE Exception handling.""" test = 0 - __EXECUTE_TIMEOUT_SEC=0.025 - __TIME_SLEEP = 0.05 # Sleep time for loops + __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.""" diff --git a/flexbe_core/test/test_logger.py b/flexbe_core/test/test_logger.py index a3b14e5..630d023 100644 --- a/flexbe_core/test/test_logger.py +++ b/flexbe_core/test/test_logger.py @@ -45,8 +45,8 @@ class TestLogger(unittest.TestCase): """Test FlexBE Logger handling.""" test = 0 - __EXECUTE_TIMEOUT_SEC=0.025 - __TIME_SLEEP = 0.05 # Sleep time for loops + __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.""" diff --git a/flexbe_core/test/test_proxies.py b/flexbe_core/test/test_proxies.py index 560e337..261191a 100755 --- a/flexbe_core/test/test_proxies.py +++ b/flexbe_core/test/test_proxies.py @@ -54,8 +54,8 @@ class TestProxies(unittest.TestCase): """Test the FlexBE proxies.""" test = 0 - __EXECUTE_TIMEOUT_SEC=0.025 - __TIME_SLEEP = 0.05 # Sleep time for loops + __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.""" diff --git a/flexbe_onboard/tests/test_onboard.py b/flexbe_onboard/tests/test_onboard.py index 858b5b4..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,7 +224,8 @@ def test_onboard_behaviors(self): behavior_logs = [] # Wait for published message - end_time = time.time() + 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) @@ -237,6 +238,7 @@ def test_onboard_behaviors(self): 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) From b2b60fb15cde33c00b6194357427e0ec25d176db Mon Sep 17 00:00:00 2001 From: David Conner Date: Mon, 23 Sep 2024 14:51:05 -0400 Subject: [PATCH 16/23] fix typo in input_action_server --- flexbe_input/flexbe_input/input_action_server.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flexbe_input/flexbe_input/input_action_server.py b/flexbe_input/flexbe_input/input_action_server.py index aa3a51a..f49934c 100644 --- a/flexbe_input/flexbe_input/input_action_server.py +++ b/flexbe_input/flexbe_input/input_action_server.py @@ -62,7 +62,7 @@ def run(self): 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 ...') From 81fd3a35154b9972cc86bd34cf8bef5d36021872 Mon Sep 17 00:00:00 2001 From: David Conner Date: Wed, 25 Sep 2024 21:47:21 -0400 Subject: [PATCH 17/23] report all states in deep_states including containers and finished states; re-request outcome on sync request; notify skipped for containers --- .../flexbe_core/core/concurrency_container.py | 34 ++++++++++--------- .../flexbe_core/core/lockable_state.py | 1 - .../flexbe_core/core/operatable_state.py | 3 +- .../core/operatable_state_machine.py | 15 ++++++++ .../core/preemptable_state_machine.py | 11 ++++++ .../flexbe_core/core/priority_container.py | 14 ++++++++ flexbe_core/flexbe_core/core/state_machine.py | 8 ++--- flexbe_mirror/flexbe_mirror/flexbe_mirror.py | 33 ++++++++++++++---- .../flexbe_mirror/mirror_state_machine.py | 9 +++-- 9 files changed, 95 insertions(+), 33 deletions(-) diff --git a/flexbe_core/flexbe_core/core/concurrency_container.py b/flexbe_core/flexbe_core/core/concurrency_container.py index 04fbe94..33c85e5 100644 --- a/flexbe_core/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/flexbe_core/core/concurrency_container.py @@ -37,6 +37,7 @@ from flexbe_core.core.event_state import EventState 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 @@ -308,23 +309,24 @@ def get_deep_states(self): 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/lockable_state.py b/flexbe_core/flexbe_core/core/lockable_state.py index 0fe2c72..cb430f3 100644 --- a/flexbe_core/flexbe_core/core/lockable_state.py +++ b/flexbe_core/flexbe_core/core/lockable_state.py @@ -99,7 +99,6 @@ def _lockable_execute(self, *args, **kwargs): def _execute_lock(self, target): """Execute lock.""" - if target in (self.state_id, 0): target = self.state_id found_target = True diff --git a/flexbe_core/flexbe_core/core/operatable_state.py b/flexbe_core/flexbe_core/core/operatable_state.py index 1fc9a01..9f805ec 100644 --- a/flexbe_core/flexbe_core/core/operatable_state.py +++ b/flexbe_core/flexbe_core/core/operatable_state.py @@ -85,7 +85,8 @@ 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: - # Logger.localinfo(f"controlled State '{self.name}' from '{self.path}'permitting outcome '{outcome}' {self.__class__.__name__}") + # Logger.localinfo(f"controlled State '{self.name}' from '{self.path}'" + # f"permitting outcome '{outcome}' {self.__class__.__name__}") self._force_transition = False return outcome diff --git a/flexbe_core/flexbe_core/core/operatable_state_machine.py b/flexbe_core/flexbe_core/core/operatable_state_machine.py index 0c28df2..e427b1f 100644 --- a/flexbe_core/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/flexbe_core/core/operatable_state_machine.py @@ -278,6 +278,21 @@ def process_sync_request(self): 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)) diff --git a/flexbe_core/flexbe_core/core/preemptable_state_machine.py b/flexbe_core/flexbe_core/core/preemptable_state_machine.py index 684d942..03a1396 100644 --- a/flexbe_core/flexbe_core/core/preemptable_state_machine.py +++ b/flexbe_core/flexbe_core/core/preemptable_state_machine.py @@ -143,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_machine.py b/flexbe_core/flexbe_core/core/state_machine.py index 230f4cc..439c424 100644 --- a/flexbe_core/flexbe_core/core/state_machine.py +++ b/flexbe_core/flexbe_core/core/state_machine.py @@ -152,8 +152,8 @@ def on_enter(self, userdata): 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__})") + # 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.""" @@ -234,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_mirror/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py index fdce862..fb47b4e 100644 --- a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py @@ -129,7 +129,7 @@ def __init__(self): 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, 2) + 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 ...') @@ -521,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 * '=') @@ -539,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: @@ -650,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: @@ -718,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 diff --git a/flexbe_mirror/flexbe_mirror/mirror_state_machine.py b/flexbe_mirror/flexbe_mirror/mirror_state_machine.py index a28d699..23efa41 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_state_machine.py +++ b/flexbe_mirror/flexbe_mirror/mirror_state_machine.py @@ -147,10 +147,9 @@ def spin(self, start_time, state_map): 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 @@ -243,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.""" From 73904d14039b34568c07ba17f43fa2cdca60db21 Mon Sep 17 00:00:00 2001 From: David Conner Date: Wed, 25 Sep 2024 21:49:19 -0400 Subject: [PATCH 18/23] rename to flexbe_status_listener with heartbeat and sync updates; use latched state_map topic --- .../flexbe_onboard/flexbe_onboard.py | 4 +- flexbe_widget/CMakeLists.txt | 2 +- flexbe_widget/bin/flexbe_outcome_listener | 6 -- flexbe_widget/bin/flexbe_status_listener | 6 ++ .../flexbe_widget/behavior_launcher.py | 6 +- ..._listener.py => flexbe_status_listener.py} | 88 +++++++++++++------ 6 files changed, 75 insertions(+), 37 deletions(-) delete mode 100644 flexbe_widget/bin/flexbe_outcome_listener create mode 100755 flexbe_widget/bin/flexbe_status_listener rename flexbe_widget/flexbe_widget/{flexbe_outcome_listener.py => flexbe_status_listener.py} (57%) diff --git a/flexbe_onboard/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py index d6172f1..0508b7e 100644 --- a/flexbe_onboard/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py @@ -93,9 +93,11 @@ def __init__(self): # only at onboard level self._heartbeat_pub = self.create_publisher(BehaviorSync, Topics._ONBOARD_HEARTBEAT_TOPIC, 10) self._status_pub = self.create_publisher(BEStatus, Topics._ONBOARD_STATUS_TOPIC, 10) - self._state_map_pub = self.create_publisher(StateMapMsg, Topics._STATE_MAP_TOPIC, 2) + # Latch state map so we can retreive 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) diff --git a/flexbe_widget/CMakeLists.txt b/flexbe_widget/CMakeLists.txt index f216901..1dcc9dc 100644 --- a/flexbe_widget/CMakeLists.txt +++ b/flexbe_widget/CMakeLists.txt @@ -11,7 +11,7 @@ install(PROGRAMS bin/breakpoint bin/create_repo bin/evaluate_logs - bin/flexbe_outcome_listener + bin/flexbe_status_listener DESTINATION lib/${PROJECT_NAME} ) diff --git a/flexbe_widget/bin/flexbe_outcome_listener b/flexbe_widget/bin/flexbe_outcome_listener deleted file mode 100644 index 34e9ec2..0000000 --- a/flexbe_widget/bin/flexbe_outcome_listener +++ /dev/null @@ -1,6 +0,0 @@ -#!/usr/bin/env python3 - -from flexbe_widget.flexbe_outcome_listener import flexbe_outcome_listener_main - -if __name__ == '__main__': - flexbe_outcome_listener_main() 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_launcher.py b/flexbe_widget/flexbe_widget/behavior_launcher.py index 082e88b..eacfffc 100644 --- a/flexbe_widget/flexbe_widget/behavior_launcher.py +++ b/flexbe_widget/flexbe_widget/behavior_launcher.py @@ -50,6 +50,7 @@ import rclpy from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSProfile from rosidl_runtime_py import get_interface_path @@ -78,7 +79,10 @@ def __init__(self): self._status_pub = self.create_publisher(BEStatus, Topics._ONBOARD_STATUS_TOPIC, 100) self._mirror_pub = self.create_publisher(ContainerStructure, Topics._MIRROR_STRUCTURE_TOPIC, 100) self._heartbeat_pub = self.create_publisher(Int32, Topics._LAUNCHER_HEARTBEAT_TOPIC, 2) - self._state_map_pub = self.create_publisher(StateMapMsg, Topics._STATE_MAP_OCS_TOPIC, 2) + + # Latch state map so we can retreive 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) diff --git a/flexbe_widget/flexbe_widget/flexbe_outcome_listener.py b/flexbe_widget/flexbe_widget/flexbe_status_listener.py similarity index 57% rename from flexbe_widget/flexbe_widget/flexbe_outcome_listener.py rename to flexbe_widget/flexbe_widget/flexbe_status_listener.py index a9d3190..424f126 100644 --- a/flexbe_widget/flexbe_widget/flexbe_outcome_listener.py +++ b/flexbe_widget/flexbe_widget/flexbe_status_listener.py @@ -28,52 +28,83 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import rclpy -from rclpy.node import Node - from flexbe_core.core import StateMap, Topics -class FlexbeOutcomeListener(Node): - """Simple lister to echo FlexBE state machine information.""" - def __init__(self): - super().__init__('flexbe_outcome_listener') - +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSProfile - # Suppress the node name and severity information (ChatGPT led me astray) - # import logging - # formatter = logging.Formatter('[%(asctime)s] %(message)s') - # console_handler = logging.StreamHandler() - # console_handler.setFormatter(formatter) - # self.get_logger().handlers.clear() - # self.get_logger().addHandler(console_handler) +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._state_map_sub # prevent unused variable warning + self._outcome_sub # prevent unused variable warning - self._state_map_sub = self.create_subscription( - Topics._topic_types[Topics._STATE_MAP_OCS_TOPIC], - Topics._STATE_MAP_OCS_TOPIC, - self._state_map_callback, + self._heartbeat_sub = self.create_subscription( + Topics._topic_types[Topics._ONBOARD_HEARTBEAT_TOPIC], + Topics._ONBOARD_HEARTBEAT_TOPIC, + self._heartbeat_callback, 10) - self._state_map_sub # prevent unused variable warning + 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}") + 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}") + 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}") + 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): @@ -82,13 +113,14 @@ def _state_map_callback(self, msg): self._state_map = state_map -def flexbe_outcome_listener_main(args=None): +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 informatio from state map.') + print(' Run this before starting behavior to get full state path information from state map.') rclpy.init(args=args) - outcomes = FlexbeOutcomeListener() + outcomes = FlexbeStatusListener() rclpy.spin(outcomes) @@ -100,4 +132,4 @@ def flexbe_outcome_listener_main(args=None): if __name__ == '__main__': - flexbe_outcome_listener_main() \ No newline at end of file + flexbe_status_listener_main() From 39beaaa7e00cbf4b3b076071e86caaf07ab94efa Mon Sep 17 00:00:00 2001 From: David Conner Date: Wed, 25 Sep 2024 21:49:31 -0400 Subject: [PATCH 19/23] clean up --- flexbe_core/test/test_core.py | 12 ++++++------ flexbe_core/test/test_exceptions.py | 2 +- flexbe_core/test/test_exceptions_spin.py | 4 ++-- flexbe_core/test/test_logger.py | 6 +++--- flexbe_core/test/test_proxies.py | 2 +- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/flexbe_core/test/test_core.py b/flexbe_core/test/test_core.py index d73e3ab..af30d59 100755 --- a/flexbe_core/test/test_core.py +++ b/flexbe_core/test/test_core.py @@ -44,7 +44,7 @@ import rclpy from rclpy.executors import MultiThreadedExecutor -from std_msgs.msg import Bool, Empty, Int32, String, UInt32 +from std_msgs.msg import Bool, Empty, Int32, UInt32 class CoreTestState(EventState): @@ -144,7 +144,7 @@ def setUp(self): def tearDown(self): """Tear down the TestCore test.""" self.node.get_logger().info(' shutting down core test %d ... ' % (self.test)) - for _ in range(int(0.25*TestCore.__LOOP_COUNT)): + 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=TestCore.__EXECUTE_TIMEOUT_SEC) @@ -161,7 +161,7 @@ def tearDown(self): # Kill it with fire to make sure not stray published topics are available rclpy.shutdown(context=self.context) - time.sleep(TestCore.__TIME_SLEEP*2) + time.sleep(TestCore.__TIME_SLEEP * 2) def _create(self): """Create the test.""" @@ -215,9 +215,9 @@ def _execute_state(self, state): 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}") + self.node.get_logger().info(f'{exc}') import traceback - self.node.get_logger().info(f"{traceback.format_exc()}") + self.node.get_logger().info(f'{traceback.format_exc()}') def assertMessage(self, sub, topic, msg, timeout=1): """Check message.""" @@ -748,7 +748,7 @@ def execute(self, userdata): 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 + 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'}, diff --git a/flexbe_core/test/test_exceptions.py b/flexbe_core/test/test_exceptions.py index 8a072e9..4aefd02 100755 --- a/flexbe_core/test/test_exceptions.py +++ b/flexbe_core/test/test_exceptions.py @@ -82,7 +82,7 @@ def tearDown(self): # Kill it with fire to make sure not stray published topics are available rclpy.shutdown(context=self.context) - time.sleep(TestExceptions.__TIME_SLEEP*2) + time.sleep(TestExceptions.__TIME_SLEEP * 2) def test_invalid_outcome(self): """Test invalid outcome.""" diff --git a/flexbe_core/test/test_exceptions_spin.py b/flexbe_core/test/test_exceptions_spin.py index fca7b03..2edd2c9 100644 --- a/flexbe_core/test/test_exceptions_spin.py +++ b/flexbe_core/test/test_exceptions_spin.py @@ -69,7 +69,7 @@ def setUp(self): 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) + 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() @@ -84,7 +84,7 @@ def tearDown(self): # Kill it with fire to make sure not stray published topics are available rclpy.shutdown(context=self.context) - time.sleep(TestExceptionsSpin.__TIME_SLEEP*2) + time.sleep(TestExceptionsSpin.__TIME_SLEEP * 2) def test_invalid_outcome(self): """Test invalid outcome.""" diff --git a/flexbe_core/test/test_logger.py b/flexbe_core/test/test_logger.py index 630d023..17a1526 100644 --- a/flexbe_core/test/test_logger.py +++ b/flexbe_core/test/test_logger.py @@ -81,7 +81,7 @@ def tearDown(self): # Kill it with fire to make sure not stray published topics are available rclpy.shutdown(context=self.context) - time.sleep(TestLogger.__TIME_SLEEP*2) + time.sleep(TestLogger.__TIME_SLEEP * 2) def test_throttle_logger_one(self): """Test throttle logger one.""" @@ -202,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=TestLogger.__EXECUTE_TIMEOUT_SEC*0.5) + 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) @@ -247,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=TestLogger.__EXECUTE_TIMEOUT_SEC*.5) + 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 261191a..a08659e 100755 --- a/flexbe_core/test/test_proxies.py +++ b/flexbe_core/test/test_proxies.py @@ -93,7 +93,7 @@ def tearDown(self): # Kill it with fire to make sure not stray published topics are available rclpy.shutdown(context=self.context) - time.sleep(TestProxies.__TIME_SLEEP*5) + time.sleep(TestProxies.__TIME_SLEEP * 5) def test_publish_subscribe(self): """Test publish and subscribe.""" From c57c7fd8aff501600244624edf55bc52380787ed Mon Sep 17 00:00:00 2001 From: David Conner Date: Wed, 25 Sep 2024 23:05:42 -0400 Subject: [PATCH 20/23] exception inside concurrency will get re-thrown to preempt behavior --- flexbe_core/flexbe_core/core/concurrency_container.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/flexbe_core/flexbe_core/core/concurrency_container.py b/flexbe_core/flexbe_core/core/concurrency_container.py index 33c85e5..c30a299 100644 --- a/flexbe_core/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/flexbe_core/core/concurrency_container.py @@ -35,6 +35,7 @@ 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 @@ -259,11 +260,17 @@ def _execute_single_state(self, state, force_exit=False): else: 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 From 4929cd19931f83a494d331a608873ca805a1cbf1 Mon Sep 17 00:00:00 2001 From: David Conner Date: Thu, 26 Sep 2024 16:49:34 -0400 Subject: [PATCH 21/23] codespell clean up --- README.md | 4 ++-- flexbe_core/CHANGELOG.rst | 2 +- flexbe_core/flexbe_core/behavior_library.py | 6 +++--- flexbe_core/flexbe_core/core/event_state.py | 2 +- flexbe_core/flexbe_core/core/operatable_state_machine.py | 4 ++-- flexbe_core/flexbe_core/core/state.py | 2 +- flexbe_core/flexbe_core/proxy/proxy_action_client.py | 2 +- flexbe_core/flexbe_core/proxy/proxy_publisher.py | 2 +- flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py | 2 +- flexbe_core/flexbe_core/proxy/proxy_transform_listener.py | 2 +- flexbe_core/test/flexbe_logger_test.py | 2 +- flexbe_core/test/test_logger.py | 2 +- flexbe_core/test/test_proxies.py | 2 +- flexbe_input/flexbe_input/complex_action_server.py | 6 +++--- flexbe_input/flexbe_input/input_action_server.py | 2 +- flexbe_mirror/flexbe_mirror/flexbe_mirror.py | 2 +- flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py | 2 +- flexbe_mirror/flexbe_mirror/mirror_state.py | 2 +- flexbe_msgs/action/BehaviorInput.action | 2 +- flexbe_msgs/action/BehaviorSynthesis.action | 2 +- flexbe_msgs/msg/Container.msg | 2 +- flexbe_onboard/flexbe_onboard/flexbe_onboard.py | 4 ++-- flexbe_states/flexbe_states/input_state.py | 4 ++-- flexbe_states/flexbe_states/selection_state.py | 2 +- flexbe_widget/flexbe_widget/behavior_launcher.py | 2 +- 25 files changed, 33 insertions(+), 33 deletions(-) diff --git a/README.md b/README.md index 2fa9050..3c650e1 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ Jazzy ![ROS Build Farm](https://build.ros2.org/job/Jdev__flexbe_behavior_engine_ Rolling ![ROS Build Farm](https://build.ros2.org/job/Rdev__flexbe_behavior_engine__ubuntu_noble_amd64/badge/icon) -> Note: This version 4+ breaks compatability with the FlexBE App. You must use the FlexBE WebUI [flexbe_webui](https://github.com/FlexBE/flexbe_webui.git) now. +> 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 @@ -58,7 +58,7 @@ from the `${WORKSPACE_ROOT}/src` folder will create: 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 compatability with the older FlexBE App and now requires use of the FlexBE WebUI tool. +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: diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index ff05188..800e9df 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -336,7 +336,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/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/event_state.py b/flexbe_core/flexbe_core/core/event_state.py index ed3c69e..6e3eb47 100644 --- a/flexbe_core/flexbe_core/core/event_state.py +++ b/flexbe_core/flexbe_core/core/event_state.py @@ -106,7 +106,7 @@ def _event_execute(self, *args, **kwargs): repeat = True if repeat or outcome is not None: - # As this is currently coded, a repeat command will immedately halt + # 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) diff --git a/flexbe_core/flexbe_core/core/operatable_state_machine.py b/flexbe_core/flexbe_core/core/operatable_state_machine.py index e427b1f..9f2465b 100644 --- a/flexbe_core/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/flexbe_core/core/operatable_state_machine.py @@ -214,7 +214,7 @@ def _execute_current_state(self): # 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 @@ -271,7 +271,7 @@ 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 diff --git a/flexbe_core/flexbe_core/core/state.py b/flexbe_core/flexbe_core/core/state.py index 080a9c6..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) diff --git a/flexbe_core/flexbe_core/proxy/proxy_action_client.py b/flexbe_core/flexbe_core/proxy/proxy_action_client.py index 5ff19f7..2248673 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_action_client.py +++ b/flexbe_core/flexbe_core/proxy/proxy_action_client.py @@ -93,7 +93,7 @@ def __init__(self, topics=None, wait_duration=1.0): 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 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_subscriber_cached.py b/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py index 7806cc6..0a64a1d 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py +++ b/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py @@ -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/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_logger.py b/flexbe_core/test/test_logger.py index 17a1526..f9a4da6 100644 --- a/flexbe_core/test/test_logger.py +++ b/flexbe_core/test/test_logger.py @@ -124,7 +124,7 @@ 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) diff --git a/flexbe_core/test/test_proxies.py b/flexbe_core/test/test_proxies.py index a08659e..b200bb6 100755 --- a/flexbe_core/test/test_proxies.py +++ b/flexbe_core/test/test_proxies.py @@ -345,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/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 f49934c..2f0a170 100644 --- a/flexbe_input/flexbe_input/input_action_server.py +++ b/flexbe_input/flexbe_input/input_action_server.py @@ -110,7 +110,7 @@ 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 diff --git a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py index fb47b4e..2f2298a 100644 --- a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py @@ -277,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)}') diff --git a/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py b/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py index 505ecad..0108e69 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py +++ b/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py @@ -148,5 +148,5 @@ def get_deep_states(self): 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})") 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 7558654..0d29314 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_state.py +++ b/flexbe_mirror/flexbe_mirror/mirror_state.py @@ -64,7 +64,7 @@ def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy): 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 diff --git a/flexbe_msgs/action/BehaviorInput.action b/flexbe_msgs/action/BehaviorInput.action index 74b42a8..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) 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_onboard/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py index 0508b7e..f89c41e 100644 --- a/flexbe_onboard/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py @@ -94,7 +94,7 @@ 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 retreive later if desired + # 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) @@ -161,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]) diff --git a/flexbe_states/flexbe_states/input_state.py b/flexbe_states/flexbe_states/input_state.py index 85e74f3..6b13ac1 100644 --- a/flexbe_states/flexbe_states/input_state.py +++ b/flexbe_states/flexbe_states/input_state.py @@ -136,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}'") diff --git a/flexbe_states/flexbe_states/selection_state.py b/flexbe_states/flexbe_states/selection_state.py index c52b803..586ca76 100644 --- a/flexbe_states/flexbe_states/selection_state.py +++ b/flexbe_states/flexbe_states/selection_state.py @@ -129,7 +129,7 @@ def on_enter(self, userdata): Logger.localwarn(msg) return - # Retrive the goal for the BehaviorInput Action. + # 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}") diff --git a/flexbe_widget/flexbe_widget/behavior_launcher.py b/flexbe_widget/flexbe_widget/behavior_launcher.py index eacfffc..1072cc9 100644 --- a/flexbe_widget/flexbe_widget/behavior_launcher.py +++ b/flexbe_widget/flexbe_widget/behavior_launcher.py @@ -80,7 +80,7 @@ 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 retreive later if desired + # 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) From 7c46fa92312e7bef8a8c4377b504e2e3733ef6a8 Mon Sep 17 00:00:00 2001 From: David Conner Date: Thu, 26 Sep 2024 20:16:06 -0400 Subject: [PATCH 22/23] update changelogs --- flexbe_behavior_engine/CHANGELOG.rst | 3 +++ flexbe_core/CHANGELOG.rst | 10 ++++++++++ flexbe_input/CHANGELOG.rst | 5 +++++ flexbe_mirror/CHANGELOG.rst | 7 +++++++ flexbe_mirror/flexbe_mirror/flexbe_mirror.py | 4 ++-- flexbe_msgs/CHANGELOG.rst | 4 ++++ flexbe_onboard/CHANGELOG.rst | 8 ++++++++ flexbe_states/CHANGELOG.rst | 4 ++++ flexbe_testing/CHANGELOG.rst | 3 +++ flexbe_widget/CHANGELOG.rst | 6 ++++++ 10 files changed, 52 insertions(+), 2 deletions(-) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 09ac6b6..767ac6a 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2024-08-24) ------------------ * Version 4.0.0 release using state_id for communication diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index 800e9df..a64f7c2 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index 660d09c..6ec8bc0 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 diff --git a/flexbe_mirror/CHANGELOG.rst b/flexbe_mirror/CHANGELOG.rst index 9f93459..633a4d1 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 diff --git a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py index 2f2298a..f503f1d 100644 --- a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py @@ -87,7 +87,7 @@ def __init__(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 @@ -103,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) diff --git a/flexbe_msgs/CHANGELOG.rst b/flexbe_msgs/CHANGELOG.rst index 58a2991..4de68e3 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* codespell clean up + 4.0.0 (2024-08-24) ------------------ * Version 4.0.0 release using state_id for communication diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index 3257d5f..bdfc988 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index 48f161c..fb98eaf 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* codespell clean up + 4.0.0 (2024-08-24) ------------------ * Version 4.0.0 release using state_id for communication diff --git a/flexbe_testing/CHANGELOG.rst b/flexbe_testing/CHANGELOG.rst index 8136d81..eff9dd1 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2024-08-24) ------------------ * Version 4.0.0 release using state_id for communication diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index 004d6ac..b55eaaf 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 From 92abd9c540db1053a25de7e76a0c0a2db786e80a Mon Sep 17 00:00:00 2001 From: David Conner Date: Thu, 26 Sep 2024 20:18:27 -0400 Subject: [PATCH 23/23] 4.0.1 --- flexbe_behavior_engine/CHANGELOG.rst | 4 ++-- flexbe_behavior_engine/package.xml | 2 +- flexbe_core/CHANGELOG.rst | 4 ++-- flexbe_core/package.xml | 2 +- flexbe_core/setup.py | 2 +- flexbe_input/CHANGELOG.rst | 4 ++-- flexbe_input/package.xml | 2 +- flexbe_input/setup.py | 2 +- flexbe_mirror/CHANGELOG.rst | 4 ++-- flexbe_mirror/package.xml | 2 +- flexbe_mirror/setup.py | 2 +- flexbe_msgs/CHANGELOG.rst | 4 ++-- flexbe_msgs/package.xml | 2 +- flexbe_onboard/CHANGELOG.rst | 4 ++-- flexbe_onboard/package.xml | 2 +- flexbe_onboard/setup.py | 2 +- flexbe_states/CHANGELOG.rst | 4 ++-- flexbe_states/package.xml | 2 +- flexbe_states/setup.py | 2 +- flexbe_testing/CHANGELOG.rst | 4 ++-- flexbe_testing/package.xml | 2 +- flexbe_widget/CHANGELOG.rst | 4 ++-- flexbe_widget/package.xml | 2 +- 23 files changed, 32 insertions(+), 32 deletions(-) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 767ac6a..17da07b 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.1 (2024-09-26) +------------------ 4.0.0 (2024-08-24) ------------------ diff --git a/flexbe_behavior_engine/package.xml b/flexbe_behavior_engine/package.xml index c9f8980..eb3c2ab 100644 --- a/flexbe_behavior_engine/package.xml +++ b/flexbe_behavior_engine/package.xml @@ -1,7 +1,7 @@ flexbe_behavior_engine - 4.0.0 + 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 a64f7c2..0b5f956 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/flexbe_core/package.xml b/flexbe_core/package.xml index 16b706b..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 - 4.0.0 + 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 cd985fa..4874d3a 100644 --- a/flexbe_core/setup.py +++ b/flexbe_core/setup.py @@ -6,7 +6,7 @@ setup( name=PACKAGE_NAME, - version='4.0.0', + version='4.0.1', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]), diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index 6ec8bc0..bc72937 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.1 (2024-09-26) +------------------ * codespell clean up * fix typo in input_action_server diff --git a/flexbe_input/package.xml b/flexbe_input/package.xml index 00654dc..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 - 4.0.0 + 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 0736766..85d9e6d 100644 --- a/flexbe_input/setup.py +++ b/flexbe_input/setup.py @@ -6,7 +6,7 @@ setup( name=PACKAGE_NAME, - version='4.0.0', + 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 633a4d1..5046fb5 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/flexbe_mirror/package.xml b/flexbe_mirror/package.xml index addf993..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 - 4.0.0 + 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 c253853..2c84118 100644 --- a/flexbe_mirror/setup.py +++ b/flexbe_mirror/setup.py @@ -7,7 +7,7 @@ setup( name=PACKAGE_NAME, - version='4.0.0', + 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 4de68e3..545d6fa 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.1 (2024-09-26) +------------------ * codespell clean up 4.0.0 (2024-08-24) diff --git a/flexbe_msgs/package.xml b/flexbe_msgs/package.xml index eaf914b..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 - 4.0.0 + 4.0.1 flexbe_msgs provides the messages used by FlexBE. diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index bdfc988..6c095dc 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.1 (2024-09-26) +------------------ * codespell clean up * rename to flexbe_status_listener with heartbeat and sync updates * use latched state_map topic diff --git a/flexbe_onboard/package.xml b/flexbe_onboard/package.xml index 4203527..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 - 4.0.0 + 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 b4dfd41..e3a9b24 100644 --- a/flexbe_onboard/setup.py +++ b/flexbe_onboard/setup.py @@ -9,7 +9,7 @@ setup( name=package_name, - version='4.0.0', + version='4.0.1', packages=find_packages(), data_files=[ (os.path.join('share', package_name), glob('launch/*.launch.py')), diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index fb98eaf..510e410 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.1 (2024-09-26) +------------------ * codespell clean up 4.0.0 (2024-08-24) diff --git a/flexbe_states/package.xml b/flexbe_states/package.xml index a862fba..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 - 4.0.0 + 4.0.1 flexbe_states provides a collection of common generic predefined states. diff --git a/flexbe_states/setup.py b/flexbe_states/setup.py index 7dce138..114bdc5 100644 --- a/flexbe_states/setup.py +++ b/flexbe_states/setup.py @@ -9,7 +9,7 @@ setup( name=PACKAGE_NAME, - version='4.0.0', + 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 eff9dd1..0b405f3 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.1 (2024-09-26) +------------------ 4.0.0 (2024-08-24) ------------------ diff --git a/flexbe_testing/package.xml b/flexbe_testing/package.xml index 5d93ea4..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 - 4.0.0 + 4.0.1 flexbe_testing provides a framework for unit testing states. diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index b55eaaf..a9fc6bc 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.1 (2024-09-26) +------------------ * codespell clean up * rename to flexbe_status_listener with heartbeat and sync updates * use latched state_map topic diff --git a/flexbe_widget/package.xml b/flexbe_widget/package.xml index 60cdf0a..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 - 4.0.0 + 4.0.1 flexbe_widget implements some smaller scripts for the behavior engine.