From 644a892996ecbbabdb1031862c0fc3da1d0b1f3e Mon Sep 17 00:00:00 2001
From: VladimirVincan <vladimirvincan@gmail.com>
Date: Mon, 21 Nov 2022 17:08:42 +0100
Subject: [PATCH] Working on debug and use_localization options. Not finished.

---
 mep3_bringup/launch/simulation_launch.py      |  1 +
 .../launch/localization_launch.py             | 59 +++++++++++--------
 mep3_simulation/launch/simulation_launch.py   |  4 +-
 3 files changed, 36 insertions(+), 28 deletions(-)

diff --git a/mep3_bringup/launch/simulation_launch.py b/mep3_bringup/launch/simulation_launch.py
index f64cfc988..dd73fdcab 100644
--- a/mep3_bringup/launch/simulation_launch.py
+++ b/mep3_bringup/launch/simulation_launch.py
@@ -18,6 +18,7 @@ def generate_launch_description():
     color = LaunchConfiguration('color', default='purple')
     use_opponents = LaunchConfiguration('opponents', default=False)
     namespace = LaunchConfiguration('namespace', default='big')
+    use_localization = LaunchConfiguration('localization', default=False)
 
     set_color_action = SetEnvironmentVariable('MEP3_COLOR', color)
     set_use_opponents = SetEnvironmentVariable('MEP3_OPPONENTS', use_opponents)
diff --git a/mep3_localization/launch/localization_launch.py b/mep3_localization/launch/localization_launch.py
index e4a31d50c..dfb127411 100644
--- a/mep3_localization/launch/localization_launch.py
+++ b/mep3_localization/launch/localization_launch.py
@@ -16,6 +16,8 @@
 
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
 
 
 def generate_launch_description():
@@ -25,32 +27,37 @@ def generate_launch_description():
 
     TODO: add debug option for static marker positions
     """
+    debug = LaunchConfiguration('debug', default=False)
     return LaunchDescription([
-        # Node(package='tf2_ros',
-        #      executable='static_transform_publisher',
-        #      arguments=[
-        #          '-0.1', '1.50976', '1.05','-4.85921e-06', '-0.965927', '0.258816',
-        #          '1.32679e-06', 'map', 'camera_prediction'
-        #      ]),
-        # Node(package='tf2_ros',
-        #      executable='static_transform_publisher',
-        #      arguments=[
-        #          '-0.430', '0.925', '0', '0', '0', '0', '1', 'map', 'marker_[20]_static'
-        #      ]),
-        # Node(package='tf2_ros',
-        #      executable='static_transform_publisher',
-        #      arguments=[
-        #          '0.430', '0.925', '0', '0', '0', '0', '1', 'map', 'marker_[21]_static',
-        #      ]),
-        # Node(package='tf2_ros',
-        #      executable='static_transform_publisher',
-        #      arguments=[
-        #          '-0.430', '-0.925', '0', '0', '0', '0', '1', 'map', 'marker_[22]_static',
-        #      ]),
-        # Node(package='tf2_ros',
-        #      executable='static_transform_publisher',
-        #      arguments=[
-        #          '0.430', '-0.925', '0', '0', '0', '0', '1', 'map', 'marker_[23]_static',
-        #      ]),
+        Node(package='tf2_ros',
+             executable='static_transform_publisher',
+             arguments=[
+                 '-0.1', '1.50976', '1.05','-4.85921e-06', '-0.965927', '0.258816',
+                 '1.32679e-06', 'map', 'camera_prediction'
+             ])
+        Node(package='tf2_ros',
+             executable='static_transform_publisher',
+             arguments=[
+                 '-0.430', '0.925', '0', '0', '0', '0', '1', 'map', 'marker_[20]_static'
+             ],
+             condition = launch.contitions.IfCondition(debug)),
+        Node(package='tf2_ros',
+             executable='static_transform_publisher',
+             arguments=[
+                 '0.430', '0.925', '0', '0', '0', '0', '1', 'map', 'marker_[21]_static',
+             ],
+             condition = launch.contitions.IfCondition(debug)),
+        Node(package='tf2_ros',
+             executable='static_transform_publisher',
+             arguments=[
+                 '-0.430', '-0.925', '0', '0', '0', '0', '1', 'map', 'marker_[22]_static',
+             ],
+             condition = launch.contitions.IfCondition(debug)),
+        Node(package='tf2_ros',
+             executable='static_transform_publisher',
+             arguments=[
+                 '0.430', '-0.925', '0', '0', '0', '0', '1', 'map', 'marker_[23]_static',
+             ],
+             condition = launch.contitions.IfCondition(debug)),
         Node(package='mep3_localization', executable='aruco_detector'),
     ])
diff --git a/mep3_simulation/launch/simulation_launch.py b/mep3_simulation/launch/simulation_launch.py
index 4d5bf1597..af47ce649 100644
--- a/mep3_simulation/launch/simulation_launch.py
+++ b/mep3_simulation/launch/simulation_launch.py
@@ -21,13 +21,14 @@ def generate_launch_description():
         'controller_params_small',
         default=os.path.join(get_package_share_directory('mep3_bringup'),
                              'resource', 'ros2_control_small.yaml'))
+    debug = LaunchConfiguration('debug', default=False)
+    use_localization = LaunchConfiguration('localization', default=False)
 
     robot_description_big = pathlib.Path(
         os.path.join(package_dir, 'resource', 'config_big.urdf')).read_text()
     robot_description_small = pathlib.Path(
         os.path.join(package_dir, 'resource',
                      'config_small.urdf')).read_text()
-
     cam_description = pathlib.Path(
         os.path.join(package_dir, 'resource', 'config_cam.urdf')).read_text()
 
@@ -89,7 +90,6 @@ def generate_launch_description():
         ],
         additional_env={'WEBOTS_CONTROLLER_URL': 'robot_small'},
         namespace='small')
-    
 
     # Camera driver for the Central Tracking Device
     webots_cam_driver_central = Node(