diff --git a/flexbe_onboard/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py index 98dd814..0464232 100644 --- a/flexbe_onboard/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py @@ -134,10 +134,10 @@ def _version_callback(self, msg): vui = FlexbeOnboard._parse_version(msg.data) vex = FlexbeOnboard._parse_version(MIN_UI_VERSION) if vui < vex: - Logger.logwarn('FlexBE App needs to be updated!\n' + Logger.logwarn('FlexBE UI needs to be updated!\n' f'Onboard Behavior Engine requires at least version {MIN_UI_VERSION}, ' f' but you have {msg.data}\n' - 'Please update the flexbe_app software.') + 'Please update the FlexBE UI software.') @staticmethod def _parse_version(v): diff --git a/flexbe_states/flexbe_states/subscriber_state.py b/flexbe_states/flexbe_states/subscriber_state.py index 60cdf1b..fa84eff 100644 --- a/flexbe_states/flexbe_states/subscriber_state.py +++ b/flexbe_states/flexbe_states/subscriber_state.py @@ -39,17 +39,17 @@ class SubscriberState(EventState): """ Gets the latest message on the given topic and stores it to userdata. - -- topic string The topic on which should be listened. - -- msg_type type The type of messages of this topic. - -- blocking bool Blocks until a message is received. - -- clear bool Drops last message on this topic on enter + -- topic string The topic on which should be listened. + -- msg_type type The type of messages of this topic. + -- blocking bool Blocks until a message is received. + -- clear bool Drops last message on this topic on enter in order to only handle message received since this state is active. - -- qos QoSProfile A QoSProfile to apply to the subscription. + -- qos QoSProfile A QoSProfile to apply to the subscription. - #> message object Latest message on the given topic of the respective type. + #> message object Latest message on the given topic of the respective type. - <= received Message has been received and stored in userdata or state is not blocking. - <= unavailable The topic is not available when this state becomes actives. + <= received Message has been received and stored in userdata or state is not blocking. + <= unavailable The topic is not available when this state becomes actives. """ def __init__(self, topic, msg_type="", blocking=True, clear=False, qos=QOS_DEFAULT): diff --git a/flexbe_widget/flexbe_widget/behavior_launcher.py b/flexbe_widget/flexbe_widget/behavior_launcher.py index f5b3d3a..b3b5702 100644 --- a/flexbe_widget/flexbe_widget/behavior_launcher.py +++ b/flexbe_widget/flexbe_widget/behavior_launcher.py @@ -46,7 +46,8 @@ from std_msgs.msg import Int32, String -from flexbe_msgs.msg import BehaviorModification, BehaviorRequest, BehaviorSelection, BEStatus, ContainerStructure +from flexbe_msgs.msg import BehaviorModification, BehaviorRequest, BehaviorSelection +from flexbe_msgs.msg import BehaviorSync, BEStatus, ContainerStructure from flexbe_core import BehaviorLibrary, Logger, MIN_UI_VERSION from flexbe_core.core import StateMap @@ -65,6 +66,8 @@ def __init__(self): self._sub = self.create_subscription(BehaviorRequest, Topics._REQUEST_BEHAVIOR_TOPIC, self._request_callback, 100) self._version_sub = self.create_subscription(String, Topics._UI_VERSION_TOPIC, self._version_callback, 1) self._status_sub = self.create_subscription(BEStatus, Topics._ONBOARD_STATUS_TOPIC, self._status_callback, 100) + self._onboard_heartbeat_sub = self.create_subscription(BehaviorSync, Topics._ONBOARD_HEARTBEAT_TOPIC, + self._onboard_heartbeat_callback, 10) self._pub = self.create_publisher(BehaviorSelection, Topics._START_BEHAVIOR_TOPIC, 100) self._status_pub = self.create_publisher(BEStatus, Topics._ONBOARD_STATUS_TOPIC, 100) @@ -75,6 +78,7 @@ def __init__(self): # Require periodic events in case behavior is not connected to allow orderly shutdown self._heartbeat_timer = self.create_timer(2.0, self.heartbeat_timer_callback) + self._last_onboard_heartbeat = None self.get_logger().info("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors()) @@ -86,6 +90,19 @@ def heartbeat_timer_callback(self): """ self._heartbeat_pub.publish(Int32(data=self.get_clock().now().seconds_nanoseconds()[0])) + if self._last_onboard_heartbeat is None: + self.get_logger().warn(f"Behavior Launcher has NOT received update from onboard behavior engine!") + else: + elapsed = self.get_clock().now() - self._last_onboard_heartbeat + if elapsed.nanoseconds > 2e9: + self.get_logger().warn(f"Behavior Launcher is NOT receiving updates from onboard behavior engine!") + self._last_onboard_heartbeat = None + + def _onboard_heartbeat_callback(self, msg): + """Record time of last onboard heartbeat.""" + self._last_onboard_heartbeat = self.get_clock().now() + + def _status_callback(self, msg): if msg.code in [BEStatus.READY, BEStatus.FINISHED, BEStatus.FAILED, BEStatus.ERROR, BEStatus.RUNNING, BEStatus.STARTED]: self.get_logger().info(f"BE status code={msg.code} received - READY for new behavior!") @@ -201,10 +218,10 @@ def _version_callback(self, msg): vui = BehaviorLauncher._parse_version(msg.data) vex = BehaviorLauncher._parse_version(MIN_UI_VERSION) if vui < vex: - Logger.logwarn('FlexBE App needs to be updated!\n' + Logger.logwarn('FlexBE UI needs to be updated!\n' f'Behavior launcher requires at least version {MIN_UI_VERSION}, ' f' but you have {msg.data}\n' - 'Please update the flexbe_app software.') + 'Please update the FlexBE UI software.') @staticmethod def _parse_version(v): @@ -263,8 +280,12 @@ def behavior_launcher_main(node_args=None): if behavior != "": print(f"Set up behavior_launcher with '{behavior}' ...", flush=True) - for _ in range(100): + prior_clock = launcher.get_clock().now() + while launcher._last_onboard_heartbeat is None: # Let stuff get going before launching behavior request + if (launcher.get_clock().now() - prior_clock).nanoseconds > 2e9: + print(f"Waiting for onboard behavior engine to launch '{behavior}' ...", flush=True) + prior_clock = launcher.get_clock().now() executor.spin_once(timeout_sec=0.001) request = BehaviorRequest(behavior_name=behavior, autonomy_level=autonomy)