From 46ee7cc7ea66bfe88f7cb79b72c6203567463895 Mon Sep 17 00:00:00 2001
From: Norbert Schulz <github@schulznorbert.de>
Date: Wed, 28 Aug 2024 16:46:10 +0200
Subject: [PATCH] Added arguments launch_ru|dcs and log_redirect

---
 README.md            | 14 ++++++---
 launch/dcs_launch.py | 72 ++++++++++++++++++++++++++++++++++++--------
 2 files changed, 69 insertions(+), 17 deletions(-)

diff --git a/README.md b/README.md
index e75c8d3..d3497b5 100644
--- a/README.md
+++ b/README.md
@@ -57,8 +57,14 @@ build only this package (and in verbose mode) use:
 
 The `ros2_dcs_turtlesim` includes a launch configuration. Run it using
 
-        ros2 launch ros2_dcs_turtlesim dcs_launch.py [world:=<path-to-world-file>] [launch_webots:=true|false]
+        ros2 launch ros2_dcs_turtlesim dcs_launch.py [args]
 
-Running with launch_webots:=false only runs the external controllers. This is usefull after a reload
-of the world. Default is to launch webots as well.
-The default world file is is set to worlds/minimal/DcsMinimal.wbt.
\ No newline at end of file
+Supported launcher arguments:
+
+| Argument      | Default              | Description          |
+|---------------|----------------------|----------------------|
+|world|world:=worlds/minimal/DcsMinimal.wbt| Launch Webots with given world file.|
+|launch_webots|launch_webots:=true| Enable/Disable launch of Webots.|
+|launch_ru|launch_ru:=true| Enable/Disable launch of RadonUlzer controller.|
+|launch_dcs|launch_dcs:=true| Enable/Disable launch of DroidControlShip controller.|
+|log_redirect|log_redirect:=false| Enable/Disable output redirection to Webots console.|
diff --git a/launch/dcs_launch.py b/launch/dcs_launch.py
index 59ce876..0e0320c 100644
--- a/launch/dcs_launch.py
+++ b/launch/dcs_launch.py
@@ -54,12 +54,12 @@ def arg_to_bool(arg):
     return False
 
 
-def generate_launch_description():
-    """Launch all components necessary for this demo."""
+def handle_arguments(args: list[str]) -> argparse.Namespace:
+    """Process launcher specific arguments."""
     package_dir = get_package_share_directory('ros2_dcs_turtlesim')
-    wb_ros_ctrl_dir = get_package_share_directory('webots_ros2_driver')
 
     arg_parser = argparse.ArgumentParser()
+
     arg_parser.add_argument(
         '--world',
         action='store',
@@ -67,6 +67,7 @@ def generate_launch_description():
         required=False,
         default=os.path.join(package_dir, 'worlds',  'minimal', 'DcsMinimal.wbt')
     )
+
     arg_parser.add_argument(
         '--launch_webots',
         type=arg_to_bool,
@@ -74,10 +75,45 @@ def generate_launch_description():
         const=True,
         default=True,
         required=False,
+        help='Enable/disble launch of Webots process.'
+    )
+
+    arg_parser.add_argument(
+        '--launch_ru',
+        type=arg_to_bool,
+        nargs='?',
+        const=True,
+        default=True,
+        required=False,
+        help='Enable/disable launch of RadonUlzer process.'
+    )
+
+    arg_parser.add_argument(
+        '--launch_dcs',
+        type=arg_to_bool,
+        nargs='?',
+        const=True,
+        default=True,
+        required=False,
+        help='Enable/disable launch of DroidControlShip process.'
+    )
+
+    arg_parser.add_argument(
+        '--log_redirect',
+        type=arg_to_bool,
+        nargs='?',
+        const=True,
+        default=False,
+        required=False,
     )
-    launch_args = sys.argv[4:]  # 4th+ arguments are the launcher specifc arguments
-    argv = [f"--{arg.replace(':=', '=')}" for arg in launch_args]  # ros2 arg:=val tp --arg=val
-    my_args = arg_parser.parse_args(argv)
+
+    python_args = [f"--{arg.replace(':=', '=')}" for arg in args]  # ros2 arg:=val to --arg=val
+    return arg_parser.parse_args(python_args)
+
+
+def generate_launch_description():
+    """Launch all components necessary for this demo."""
+    my_args = handle_arguments(sys.argv[4:])  # 4th+  are the launcher specifc.
 
     dcs_home = os.getenv('DCS_HOME')
     ru_home = os.getenv('RU_HOME')
@@ -90,7 +126,12 @@ def generate_launch_description():
         sys.exit('RU_HOME environment variable not set.')
 
     wb_ctrl_path = gen_exe_path(
-        os.path.join(wb_ros_ctrl_dir, 'scripts', 'webots-controller'))
+        os.path.join(
+            get_package_share_directory('webots_ros2_driver'),
+            'scripts',
+            'webots-controller')
+    )
+
     if not os.path.isfile(wb_ctrl_path):
         sys.exit(f'webots_controller program not found in {wb_ctrl_path}')
 
@@ -110,11 +151,14 @@ def generate_launch_description():
         #
         webots = WebotsLauncher(world=my_args.world)
 
+    wb_controller_cmd = [wb_ctrl_path]
+    if my_args.log_redirect:
+        wb_controller_cmd += ['--stdout-redirect', '--stderr-redirect']
+
     # DroidControlship Launcher
     #
     dcs_controller = ExecuteProcess(
-        cmd=[
-            wb_ctrl_path,
+        cmd=wb_controller_cmd + [
             '--robot-name=ZumoComSystem',
             dcs_path,
             '--cfgFilePath',
@@ -126,8 +170,7 @@ def generate_launch_description():
     # RadonUlzer Launcher
     #
     ru_controller = ExecuteProcess(
-        cmd=[
-            wb_ctrl_path,
+        cmd=wb_controller_cmd + [
             '--robot-name=Zumo',
             ru_path,
         ],
@@ -150,7 +193,10 @@ def generate_launch_description():
             )
         )
 
-    actions.append(dcs_controller)
-    actions.append(ru_controller)
+    if my_args.launch_dcs:
+        actions.append(dcs_controller)
+
+    if my_args.launch_ru:
+        actions.append(ru_controller)
 
     return LaunchDescription(actions)