From ce6104f2b78b219e2ea88c9081cc2da29204134d Mon Sep 17 00:00:00 2001
From: Anish <Anishdeva17@gmail.com>
Date: Tue, 1 Aug 2023 09:15:15 -0400
Subject: [PATCH 1/7] update localization launch

---
 carma/launch/carma_src.launch.py    |  7 +++-
 carma/launch/localization.launch    | 63 -----------------------------
 carma/launch/localization.launch.py | 11 ++++-
 3 files changed, 15 insertions(+), 66 deletions(-)
 delete mode 100755 carma/launch/localization.launch

diff --git a/carma/launch/carma_src.launch.py b/carma/launch/carma_src.launch.py
index 29fcea45a2..bcdfe25246 100644
--- a/carma/launch/carma_src.launch.py
+++ b/carma/launch/carma_src.launch.py
@@ -140,6 +140,9 @@ def generate_launch_description():
     vector_map_file = LaunchConfiguration('vector_map_file')
     declare_vector_map_file = DeclareLaunchArgument(name='vector_map_file', default_value='/opt/carma/maps/vector_map.osm')
 
+    simulation_mode = LaunchConfiguration('simulation')
+    declare_simulation_mode = DeclareLaunchArgument(name='simulation', default_value = 'False', descriptionn = 'True if CARMA Platform is launched with CARLA Simulator')
+
     # Nodes
 
     transform_group = GroupAction(
@@ -178,7 +181,8 @@ def generate_launch_description():
                     'area' : area,
                     'arealist_path' : arealist_path,
                     'vector_map_file' : vector_map_file,
-                    'vehicle_calibration_dir': vehicle_calibration_dir
+                    'vehicle_calibration_dir': vehicle_calibration_dir,
+                    'simulation_mode': simulation_mode,
                 }.items()
             )
         ]
@@ -271,6 +275,7 @@ def generate_launch_description():
         declare_area,
         declare_arealist_path,
         declare_vector_map_file,
+        declare_simulation_mode,
         drivers_group,
         transform_group,
         environment_group,
diff --git a/carma/launch/localization.launch b/carma/launch/localization.launch
deleted file mode 100755
index 49205a6c89..0000000000
--- a/carma/launch/localization.launch
+++ /dev/null
@@ -1,63 +0,0 @@
-<?xml version="1.0"?>
-<!--
-  Copyright (C) 2018-2021 LEIDOS.
-
-  Licensed under the Apache License, Version 2.0 (the "License"); you may not
-  use this file except in compliance with the License. You may obtain a copy of
-  the License at
-
-  http://www.apache.org/licenses/LICENSE-2.0
-
-  Unless required by applicable law or agreed to in writing, software
-  distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
-  WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
-  License for the specific language governing permissions and limitations under
-  the License.
--->
-<!--
-	Launch file for launching the nodes in the CARMA localization stack minus drivers
--->
-<launch>
-
-  <arg name="vehicle_calibration_dir" default="$(find carma)../../CARMAVehicleCalibration/development/vehicle" doc="The directory continaing vehicle calibration type parameters"/>
-  <arg name="area" default="5x5" doc="Dimensions of the square of cells loaded at runtime using the download and arealist load types"/>
-  <arg name="arealist_path" default="arealist.txt" doc="Path to the arealist.txt file which contains the paths and dimensions of each map cell to load"/>
-  <arg name="load_type" default="noupdate" doc="Enum of the map loading approach to use. Can be 'download', 'noupdate', or 'arealist'"/> 
-  <arg name="single_pcd_path" default="pcd_map.pcd" doc="Path to the map pcd file if using the noupdate load type"/>
-  <arg name="vector_map_file" default="vector_map.osm" doc="Path to the map osm file if using the noupdate load type"/>
-  <arg name="simulation_mode" default="true" doc="True if CARMA Platform is launched with CARLA Simulator"/>
-  
-  
-  <!-- Remap any absolute topics to be relative -->
-  <remap from="/vehicle/odom" to="vehicle/odom"/>
-  <remap from="/estimate_twist" to="estimate_twist"/>
-  <remap from="/estimated_vel" to="estimated_vel"/>
-  <remap from="/estimated_vel_kmph" to="estimated_vel_kmph"/>
-  <remap from="/estimated_vel_mps" to="estimated_vel_mps"/>
-  <remap from="/filtered_points" to="filtered_points"/>
-  <remap from="/imu_raw" to="imu_raw"/>
-  <remap from="/localizer_pose" to="localizer_pose"/>
-  <remap from="/ndt_pose" to="ndt_pose"/>
-  <remap from="/ndt_reliability" to="ndt_reliability"/>
-  <remap from="/ndt_stat" to="ndt_stat"/>
-  <remap from="/pmap_stat" to="pmap_stat"/>
-  <remap from="/points_downsampler_info" to="points_downsampler_info"/>
-  <remap from="/points_map" to="points_map"/>
-  <remap from="/predict_pose" to="predict_pose"/>
-  <remap from="/predict_pose_imu" to="predict_pose_imu"/>
-  <remap from="/predict_pose_imu_odom" to="predict_pose_imu_odom"/>
-  <remap from="/predict_pose_odom" to="predict_pose_odom"/>
-  <remap from="/time_ndt_matching" to="time_ndt_matching"/>
-
-  <!-- Remap topics from external packages -->
-  <remap from="imu_raw" to="$(optenv CARMA_INTR_NS)/imu_raw"/>
-
-  <remap from="system_alert" to="/system_alert"/>
-
-  <remap from="gnss_fix_fused" to="$(optenv CARMA_INTR_NS)/gnss_fix_fused"/>
-  
-  <!-- <arg name="current_twist" default="current_twist" doc="Remapping of the twist topic used by the deadreckoner node"/>
-  <arg name="current_odom" default="current_odom" doc="Remapping of the twist topic used by the deadreckoner node"/> -->
-  <!-- Localization Package -->
-  
-</launch>
diff --git a/carma/launch/localization.launch.py b/carma/launch/localization.launch.py
index a67819e780..2d5cd32fdf 100644
--- a/carma/launch/localization.launch.py
+++ b/carma/launch/localization.launch.py
@@ -77,6 +77,8 @@ def generate_launch_description():
     vector_map_file = LaunchConfiguration('vector_map_file')
     declare_map_file = DeclareLaunchArgument(name='vector_map_file', default_value='vector_map.osm', description='Path to the map osm file if using the noupdate load type')
 
+    simulation_mode = LaunchConfiguration('simulation')
+
     gnss_to_map_convertor_container = ComposableNodeContainer(
     package='carma_ros2_utils',
     name='gnss_to_map_convertor_container',
@@ -356,7 +358,7 @@ def generate_launch_description():
         arguments=['--ros-args', '--log-level', GetLogLevel('subsystem_controllers', env_log_levels)]
     )
 
-    return LaunchDescription([
+    ld = LaunchDescription([
         declare_subsystem_controller_param_file_arg,       
         declare_load_type,
         declare_single_pcd_path,
@@ -371,6 +373,11 @@ def generate_launch_description():
         map_param_loader_container,
         pcd_map_file_loader_container,
         ndt_matching_container,
-        ekf_localizer_container,
         subsystem_controller
     ]) 
+
+    if simulation_mode is False:
+        ld.add_action(ekf_localizer_container)
+        return ld
+    else:
+        return ld
\ No newline at end of file

From 97a57029b17e314ff697753b085a0167c6bac81a Mon Sep 17 00:00:00 2001
From: Anish <Anishdeva17@gmail.com>
Date: Tue, 1 Aug 2023 10:57:10 -0400
Subject: [PATCH 2/7] fix

---
 carma/launch/carma_src.launch.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/carma/launch/carma_src.launch.py b/carma/launch/carma_src.launch.py
index bcdfe25246..dcc25033eb 100644
--- a/carma/launch/carma_src.launch.py
+++ b/carma/launch/carma_src.launch.py
@@ -141,7 +141,7 @@ def generate_launch_description():
     declare_vector_map_file = DeclareLaunchArgument(name='vector_map_file', default_value='/opt/carma/maps/vector_map.osm')
 
     simulation_mode = LaunchConfiguration('simulation')
-    declare_simulation_mode = DeclareLaunchArgument(name='simulation', default_value = 'False', descriptionn = 'True if CARMA Platform is launched with CARLA Simulator')
+    declare_simulation_mode = DeclareLaunchArgument(name='simulation', default_value = 'False', description = 'True if CARMA Platform is launched with CARLA Simulator')
 
     # Nodes
 

From 6a4043b1dedc8d92cfd40430ea52e5d55b4a3dc6 Mon Sep 17 00:00:00 2001
From: Anish <Anishdeva17@gmail.com>
Date: Tue, 1 Aug 2023 10:59:10 -0400
Subject: [PATCH 3/7] remove ros1 localization launch

---
 carma/launch/carma_src.launch | 12 ------------
 1 file changed, 12 deletions(-)

diff --git a/carma/launch/carma_src.launch b/carma/launch/carma_src.launch
index a6dfd6a3ab..1ebcb2b1b2 100755
--- a/carma/launch/carma_src.launch
+++ b/carma/launch/carma_src.launch
@@ -86,18 +86,6 @@
     </include>
   </group>
 
-  <!-- Localization Stack -->
-  <group ns="$(env CARMA_LOCZ_NS)">
-    <include file="$(find carma)/launch/localization.launch">
-      <arg name="arealist_path" value="$(arg arealist_path)"/>
-      <arg name="area" value="$(arg area)"/>
-      <arg name="load_type" value="$(arg load_type)"/>
-      <arg name="single_pcd_path" value="$(arg single_pcd_path)"/>
-      <arg name="vehicle_calibration_dir" value="$(arg vehicle_calibration_dir)"/>
-      <arg name="vector_map_file" value="$(arg vector_map_file)" />
-      <arg name="simulation_mode" value="$(arg simulation_mode)" />
-    </include>
-  </group>
 
   <!-- Environment Perception Stack -->
   <group ns="$(env CARMA_ENV_NS)">

From abf233e65c648adafb670b8e31c4af4748fe2ef3 Mon Sep 17 00:00:00 2001
From: Anish <Anishdeva17@gmail.com>
Date: Tue, 1 Aug 2023 15:32:11 -0400
Subject: [PATCH 4/7] fix

---
 carma/launch/localization.launch.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/carma/launch/localization.launch.py b/carma/launch/localization.launch.py
index 2d5cd32fdf..5efbe38502 100644
--- a/carma/launch/localization.launch.py
+++ b/carma/launch/localization.launch.py
@@ -77,7 +77,7 @@ def generate_launch_description():
     vector_map_file = LaunchConfiguration('vector_map_file')
     declare_map_file = DeclareLaunchArgument(name='vector_map_file', default_value='vector_map.osm', description='Path to the map osm file if using the noupdate load type')
 
-    simulation_mode = LaunchConfiguration('simulation')
+    simulation_mode = LaunchConfiguration('simulation_mode')
 
     gnss_to_map_convertor_container = ComposableNodeContainer(
     package='carma_ros2_utils',

From e4532da4eb6b120d137620fcda391c7aec271fc5 Mon Sep 17 00:00:00 2001
From: Anish <Anishdeva17@gmail.com>
Date: Tue, 1 Aug 2023 15:59:35 -0400
Subject: [PATCH 5/7] add ifcondition for ekf localizer launch

---
 carma/launch/localization.launch.py | 15 ++++++---------
 1 file changed, 6 insertions(+), 9 deletions(-)

diff --git a/carma/launch/localization.launch.py b/carma/launch/localization.launch.py
index 5efbe38502..e47ebbd5d1 100644
--- a/carma/launch/localization.launch.py
+++ b/carma/launch/localization.launch.py
@@ -30,6 +30,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.actions import GroupAction
 from launch_ros.actions import set_remap
+from launch.conditions import IfCondition
 
 
 def generate_launch_description():
@@ -237,6 +238,7 @@ def generate_launch_description():
     # EKF Localizer
     # Comment out to remove and change marked line in waypoint following.launch
     ekf_localizer_container = ComposableNodeContainer(
+        condition = IfCondition(simulation_mode),
         package='carma_ros2_utils',
         name='ekf_localizer_container',
         namespace=GetCurrentNamespace(),
@@ -358,7 +360,7 @@ def generate_launch_description():
         arguments=['--ros-args', '--log-level', GetLogLevel('subsystem_controllers', env_log_levels)]
     )
 
-    ld = LaunchDescription([
+    return LaunchDescription([
         declare_subsystem_controller_param_file_arg,       
         declare_load_type,
         declare_single_pcd_path,
@@ -373,11 +375,6 @@ def generate_launch_description():
         map_param_loader_container,
         pcd_map_file_loader_container,
         ndt_matching_container,
-        subsystem_controller
-    ]) 
-
-    if simulation_mode is False:
-        ld.add_action(ekf_localizer_container)
-        return ld
-    else:
-        return ld
\ No newline at end of file
+        ekf_localizer_container,
+        subsystem_controller,
+    ]) 
\ No newline at end of file

From b2c143187c64d773fa983322d019dd3bfbf5bed5 Mon Sep 17 00:00:00 2001
From: Anish <Anishdeva17@gmail.com>
Date: Tue, 1 Aug 2023 16:35:03 -0400
Subject: [PATCH 6/7] fix condition

---
 carma/launch/carma_src.launch.py    | 4 ++--
 carma/launch/localization.launch.py | 3 ++-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/carma/launch/carma_src.launch.py b/carma/launch/carma_src.launch.py
index dcc25033eb..6165f29d05 100644
--- a/carma/launch/carma_src.launch.py
+++ b/carma/launch/carma_src.launch.py
@@ -140,8 +140,8 @@ def generate_launch_description():
     vector_map_file = LaunchConfiguration('vector_map_file')
     declare_vector_map_file = DeclareLaunchArgument(name='vector_map_file', default_value='/opt/carma/maps/vector_map.osm')
 
-    simulation_mode = LaunchConfiguration('simulation')
-    declare_simulation_mode = DeclareLaunchArgument(name='simulation', default_value = 'False', description = 'True if CARMA Platform is launched with CARLA Simulator')
+    simulation_mode = LaunchConfiguration('simulation_mode')
+    declare_simulation_mode = DeclareLaunchArgument(name='simulation_mode', default_value = 'False', description = 'True if CARMA Platform is launched with CARLA Simulator')
 
     # Nodes
 
diff --git a/carma/launch/localization.launch.py b/carma/launch/localization.launch.py
index e47ebbd5d1..a66eada659 100644
--- a/carma/launch/localization.launch.py
+++ b/carma/launch/localization.launch.py
@@ -31,6 +31,7 @@
 from launch.actions import GroupAction
 from launch_ros.actions import set_remap
 from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
 
 
 def generate_launch_description():
@@ -238,7 +239,7 @@ def generate_launch_description():
     # EKF Localizer
     # Comment out to remove and change marked line in waypoint following.launch
     ekf_localizer_container = ComposableNodeContainer(
-        condition = IfCondition(simulation_mode),
+        condition=IfCondition(PythonExpression(['not ', simulation_mode])),
         package='carma_ros2_utils',
         name='ekf_localizer_container',
         namespace=GetCurrentNamespace(),

From 6f9281d76050bca1a285dade9de960968068a683 Mon Sep 17 00:00:00 2001
From: Anish <Anishdeva17@gmail.com>
Date: Tue, 1 Aug 2023 16:36:27 -0400
Subject: [PATCH 7/7] fix

---
 carma/launch/localization.launch.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/carma/launch/localization.launch.py b/carma/launch/localization.launch.py
index a66eada659..a4b891335e 100644
--- a/carma/launch/localization.launch.py
+++ b/carma/launch/localization.launch.py
@@ -377,5 +377,5 @@ def generate_launch_description():
         pcd_map_file_loader_container,
         ndt_matching_container,
         ekf_localizer_container,
-        subsystem_controller,
+        subsystem_controller
     ]) 
\ No newline at end of file