add argument to allow auto launching behavior based on initial heartb…

…eat message (instead of READY signal published every 10 seconds)

flexbe_widget/flexbe_widget/behavior_launcher.py

@@ -32,8 +32,8 @@
 """Node to launch FlexBE behaviors."""
 
 from datetime import datetime
+import argparse
 import difflib
-import getopt
 import os
 import sys
 import threading
@@ -79,6 +79,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._last_heartbeat_msg = None
 
         self.get_logger().info("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors())
 
@@ -97,11 +98,11 @@ def heartbeat_timer_callback(self):
             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()
-
+        self._last_heartbeat_msg = msg
 
     def _status_callback(self, msg):
         if msg.code in [BEStatus.READY, BEStatus.FINISHED, BEStatus.FAILED, BEStatus.ERROR, BEStatus.RUNNING, BEStatus.STARTED]:
@@ -233,44 +234,42 @@ def _parse_version(v):
         return result
 
 
-def usage():
-    print("Usage: Soon...")
+def behavior_launcher_main():
+    """Run behavior launcher."""
+    # Create the argument parser
+    parser = argparse.ArgumentParser(description='Behavior launcher for FlexBE')
 
-
-def behavior_launcher_main(node_args=None):
-    opts = None
-    args = None
+    # Add the non-ROS node arguments to the parser
+    parser.add_argument('-b', '--behavior', type=str, help='Specify the behavior to launch')
+    parser.add_argument('-a', '--autonomy', type=int, default=255, help='Specify the autonomy level')
+    parser.add_argument('-s', '--autostart', action='store_true', help='Automatically start the behavior on heartbeat')
 
     try:
-        print(sys.argv)
         stop_index = len(sys.argv)
         try:
             # Stop processing after --ros-args
             stop_index = next(i for i in range(len(sys.argv)) if sys.argv[i] == "--ros-args")
-        except Exception:  # pylint: disable=W0703
+        except StopIteration:
             pass
-        opts, args = getopt.getopt(sys.argv[1:stop_index], "hb:a:", ["help", "behavior=", "autonomy="])
-    except getopt.GetoptError as exc:
-        usage()
+
+        # Parse known arguments up to stop_index
+        behavior_args = sys.argv[1:stop_index]
+        node_args = sys.argv[stop_index:]
+        args = parser.parse_args(behavior_args)
+
+    except Exception as exc:
+        parser.print_help()
         print(exc)
-        print("Continue after exception ")
-
-    behavior = ""
-    autonomy = 255
-
-    if opts:
-        for opt, arg in opts:
-            if opt in ("-h", "--help"):
-                usage()
-                sys.exit()
-            elif opt in ("-b", "--behavior"):
-                behavior = arg
-            elif opt in ("-a", "--autonomy"):
-                autonomy = int(arg)
-    ignore_args = ['__node', '__log']  # auto-set by roslaunch
+        sys.exit(-1)
+
+    behavior = args.behavior if args.behavior else ""
+    autonomy = args.autonomy
+    auto_start = args.autostart
+
+    print(f"Behavior launcher with behavior'{behavior}' autonomy={autonomy} auto_start={auto_start}\n"
+          f"    behavior args='{behavior_args}'\n    node_args='{node_args}'", flush=True)
 
     print("Set up behavior_launcher ROS connections ...", flush=True)
-    node_args = sys.argv[stop_index:]
     rclpy.init(args=node_args,
                signal_handler_options=rclpy.signals.SignalHandlerOptions.NO)  # We will handle shutdown
 
@@ -279,38 +278,63 @@ def behavior_launcher_main(node_args=None):
     executor.add_node(launcher)
 
     if behavior != "":
-        print(f"Set up behavior_launcher with '{behavior}' ...", flush=True)
+        Logger.info(f"Set up behavior_launcher to launch '{behavior}' ({auto_start})...")
         prior_clock = launcher.get_clock().now()
         while launcher._last_onboard_heartbeat is None:
-            # Let stuff get going before launching behavior request
+            # Wait for confirmation that Onboard Behavior Engine is running
+            # 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)
+                Logger.info(f"Waiting for onboard behavior engine before launching '{behavior}' ...")
                 prior_clock = launcher.get_clock().now()
             executor.spin_once(timeout_sec=0.001)
 
+        while not launcher._ready_event.is_set():
+            # If READY signal not received
+            if launcher._last_heartbeat_msg and auto_start:
+                # After getting heart beat message,
+                # then presume ready if auto_start is set and no behavior is reported active
+                if (launcher._last_heartbeat_msg.behavior_id == 0 and
+                    len(launcher._last_heartbeat_msg.current_state_checksums) == 0):
+                        Logger.info('No behavior is currently active on startup '
+                                                   'so presume ready and start new behavior!')
+                        launcher._ready_event.set()
+                else:
+                    if (launcher.get_clock().now() - prior_clock).nanoseconds > 2e9:
+                        Logger.warning('Cannot launch behavior while prior behavior is active'
+                                                   f' (id={launcher._last_heartbeat_msg.behavior_id} ...')
+                        prior_clock = launcher.get_clock().now()
+            else:
+                # Wait for confirmed ready signal if not autostarting
+                if (launcher.get_clock().now() - prior_clock).nanoseconds > 2e9:
+                    Logger.warning(f"Waiting for onboard behavior engine ready status signal '{behavior}' ...")
+                    prior_clock = launcher.get_clock().now()
+            executor.spin_once(timeout_sec=0.001)
+
+        Logger.info('Prepare behavior launch request for Onboard behavior Engine ...')
+        ignore_args = ['__node', '__log']  # auto-set by roslaunch
         request = BehaviorRequest(behavior_name=behavior, autonomy_level=autonomy)
-        for arg in args:
+        for arg in behavior_args:
             if ':=' not in arg:
                 continue
             key, val = arg.split(':=', 1)
             if key in ignore_args:
                 continue
             request.arg_keys.append('/' + key)
             request.arg_values.append(val)
-        print(f"Add callback request for '{behavior}' ...", flush=True)
         executor.spin_once(timeout_sec=0.001)
 
         # Set up a callable as a future task so we don't block before starting to spin
         def initial_request():
             return launcher._request_callback(request)
+        Logger.info(f"Create task to start behavior '{behavior}' ...")
         future = executor.create_task(initial_request)
     else:
         future = None
 
     try:
         # Wait for ctrl-c to stop the application
         if future:
-            print("Run initial request of behavior_launcher spinner ...", flush=True)
+            Logger.info("Run initial request of behavior_launcher spinner ...")
             executor.spin_until_future_complete(future, timeout_sec=10.0)
             if future.done():
                 Logger.info("Initial behavior launcher request is sent!")