From 2550b30883d9168cbd96aed1413c8e2d1344b34e Mon Sep 17 00:00:00 2001
From: nlaury <109548200+nlaury@users.noreply.github.com>
Date: Tue, 29 Oct 2024 19:29:56 -0400
Subject: [PATCH 1/5] reorganized launch file (robot.launch)

---
 lunabot_bringup/launch/autonomy.launch | 22 +++++++++++++++
 lunabot_bringup/launch/robot.launch    | 37 ++------------------------
 lunabot_bringup/launch/slam.launch     | 12 +++++++++
 lunabot_bringup/launch/teensy.launch   |  4 +++
 4 files changed, 40 insertions(+), 35 deletions(-)
 create mode 100644 lunabot_bringup/launch/autonomy.launch
 create mode 100644 lunabot_bringup/launch/slam.launch
 create mode 100644 lunabot_bringup/launch/teensy.launch

diff --git a/lunabot_bringup/launch/autonomy.launch b/lunabot_bringup/launch/autonomy.launch
new file mode 100644
index 00000000..d2274b82
--- /dev/null
+++ b/lunabot_bringup/launch/autonomy.launch
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<launch>
+    <include if="$(arg slam)" file="$(find rtabmap_ros)/launch/slam.launch"/>
+
+    <node pkg="lunabot_control" type="effort_factory.py" name="effort_factory" output="screen"/>
+
+    <node pkg="lunabot_control" type="differential_drive_controller.py" name="differential_drive_controller" output="screen"/>
+
+    <group ns="nav">
+        <node pkg="lunabot_control" type="mpc_node" name="mpc_node" output="screen"/>
+
+        <node name="global_planner_node" pkg="lunabot_nav" type="dstar_node.py" output="screen" respawn="true">
+            <!-- <rosparam file="$(find lunabot_config)/config/rrtstar.yml" command="load"/> -->
+        </node>
+    </group>
+
+    <group ns="maps">
+        <node name="costmap_node" pkg="lunabot_perception" type="costmap_node" output="screen">
+            <rosparam file="$(find lunabot_config)/config/global_costmap.yml" command="load" ns="global_costmap" />
+        </node>
+    </group>
+</launch>
\ No newline at end of file
diff --git a/lunabot_bringup/launch/robot.launch b/lunabot_bringup/launch/robot.launch
index a20b8b17..d3616588 100644
--- a/lunabot_bringup/launch/robot.launch
+++ b/lunabot_bringup/launch/robot.launch
@@ -2,49 +2,16 @@
 <launch>
     <arg name="autonomy" default="false" />
     <arg name="record" default="true" />
-    <arg name="teensy" default="true" />
     <arg name="cameras" default="true" />
     <arg name="overhead" default="true" />
     <arg name="slam" default="true" />
     <arg name="exp_name" default="exp" />
 
-    <node if="$(arg teensy)" name="teensy_driver" pkg="lunabot_embedded" type="teensy_driver_node" output="screen"/>
-
+    <include file="$(find lunabot_bringup)/launch/teensy.launch"/>
     <include if="$(arg cameras)" file="$(find lunabot_perception)/launch/cameras.launch"/>
     <include if="$(arg overhead)" file="$(find lunabot_bringup)/launch/overhead.launch"/>
     <include if="$(arg record)" file="$(find lunabot_bringup)/launch/record.launch">
         <arg name="exp_name" value="$(arg exp_name)"/>
     </include>
-
-    <group if="$(arg autonomy)">
-
-      <include if="$(arg slam)" file="$(find rtabmap_ros)/launch/rtabmap.launch" >
-        <arg name="rtabmap_args" value="--delete_db_on_start"/>
-        <arg name="depth_topic" value="/d455_back/camera/aligned_depth_to_color/image_raw"/>
-        <arg name="rgb_topic" value="/d455_back/camera/color/image_rect_color"/>
-        <arg name="camera_info_topic" value="/d455_back/camera/color/camera_info"/>
-        <arg name="rtabmapviz" value="false" />
-        <arg name="imu_topic" value="/imu" />
-        <arg name="frame_id" value="base_link" />
-      </include>
-
-      <node pkg="lunabot_control" type="effort_factory.py" name="effort_factory" output="screen"/>
-
-      <node pkg="lunabot_control" type="differential_drive_controller.py" name="differential_drive_controller" output="screen"/>
-
-      <group ns="nav">
-          <node pkg="lunabot_control" type="mpc_node" name="mpc_node" output="screen"/>
-
-          <node name="global_planner_node" pkg="lunabot_nav" type="dstar_node.py" output="screen" respawn="true">
-              <!-- <rosparam file="$(find lunabot_config)/config/rrtstar.yml" command="load"/> -->
-          </node>
-      </group>
-
-      <group ns="maps">
-          <node name="costmap_node" pkg="lunabot_perception" type="costmap_node" output="screen">
-              <rosparam file="$(find lunabot_config)/config/global_costmap.yml" command="load" ns="global_costmap" />
-          </node>
-      </group>
-
-    </group>
+    <include if="$(arg autonomy)" file="$(find lunabot_bringup)/launch/autonomy.launch"/>
 </launch>
diff --git a/lunabot_bringup/launch/slam.launch b/lunabot_bringup/launch/slam.launch
new file mode 100644
index 00000000..b5aeefc0
--- /dev/null
+++ b/lunabot_bringup/launch/slam.launch
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+        <arg name="rtabmap_args" value="--delete_db_on_start"/>
+        <arg name="depth_topic" value="/d455_back/camera/aligned_depth_to_color/image_raw"/>
+        <arg name="rgb_topic" value="/d455_back/camera/color/image_rect_color"/>
+        <arg name="camera_info_topic" value="/d455_back/camera/color/camera_info"/>
+        <arg name="rtabmapviz" value="false" />
+        <arg name="imu_topic" value="/imu" />
+        <arg name="frame_id" value="base_link" />
+    </include>
+</launch>
\ No newline at end of file
diff --git a/lunabot_bringup/launch/teensy.launch b/lunabot_bringup/launch/teensy.launch
new file mode 100644
index 00000000..ffcb361f
--- /dev/null
+++ b/lunabot_bringup/launch/teensy.launch
@@ -0,0 +1,4 @@
+<?xml version="1.0"?>
+<launch>
+    <node name="teensy_driver" pkg="lunabot_embedded" type="teensy_driver_node" output="screen"/>
+</launch>
\ No newline at end of file

From 9508e0b3a1ce12db830ca2531fec740dda8fb2f4 Mon Sep 17 00:00:00 2001
From: nlaury <109548200+nlaury@users.noreply.github.com>
Date: Tue, 29 Oct 2024 20:03:47 -0400
Subject: [PATCH 2/5] reorganized launch file (robot.launch)

---
 lunabot_bringup/launch/autonomy.launch | 1 +
 lunabot_bringup/launch/robot.launch    | 4 +++-
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/lunabot_bringup/launch/autonomy.launch b/lunabot_bringup/launch/autonomy.launch
index d2274b82..f2ab3ffa 100644
--- a/lunabot_bringup/launch/autonomy.launch
+++ b/lunabot_bringup/launch/autonomy.launch
@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
 <launch>
+    
     <include if="$(arg slam)" file="$(find rtabmap_ros)/launch/slam.launch"/>
 
     <node pkg="lunabot_control" type="effort_factory.py" name="effort_factory" output="screen"/>
diff --git a/lunabot_bringup/launch/robot.launch b/lunabot_bringup/launch/robot.launch
index d3616588..8932d5b1 100644
--- a/lunabot_bringup/launch/robot.launch
+++ b/lunabot_bringup/launch/robot.launch
@@ -13,5 +13,7 @@
     <include if="$(arg record)" file="$(find lunabot_bringup)/launch/record.launch">
         <arg name="exp_name" value="$(arg exp_name)"/>
     </include>
-    <include if="$(arg autonomy)" file="$(find lunabot_bringup)/launch/autonomy.launch"/>
+    <include if="$(arg autonomy)" file="$(find lunabot_bringup)/launch/autonomy.launch">
+        <arg name="slam" value="$(arg slam)"/>
+    </include>
 </launch>

From 4071f9a2428d03a18e3e40af7675b8aa139a65c4 Mon Sep 17 00:00:00 2001
From: nlaury <109548200+nlaury@users.noreply.github.com>
Date: Tue, 29 Oct 2024 20:34:44 -0400
Subject: [PATCH 3/5] modified slam argument passing to autonomy.launch

---
 lunabot_bringup/launch/slam.launch | 1 +
 1 file changed, 1 insertion(+)

diff --git a/lunabot_bringup/launch/slam.launch b/lunabot_bringup/launch/slam.launch
index b5aeefc0..0627a3e6 100644
--- a/lunabot_bringup/launch/slam.launch
+++ b/lunabot_bringup/launch/slam.launch
@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="slam" default="true"/>
     <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
         <arg name="rtabmap_args" value="--delete_db_on_start"/>
         <arg name="depth_topic" value="/d455_back/camera/aligned_depth_to_color/image_raw"/>

From 876e24903ab948b975a3d8be67979d11e399401c Mon Sep 17 00:00:00 2001
From: nlaury <109548200+nlaury@users.noreply.github.com>
Date: Tue, 29 Oct 2024 20:57:50 -0400
Subject: [PATCH 4/5] corrected wrong slam argument passing

---
 lunabot_bringup/launch/autonomy.launch | 2 +-
 lunabot_bringup/launch/slam.launch     | 1 -
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/lunabot_bringup/launch/autonomy.launch b/lunabot_bringup/launch/autonomy.launch
index f2ab3ffa..c960c51e 100644
--- a/lunabot_bringup/launch/autonomy.launch
+++ b/lunabot_bringup/launch/autonomy.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-    
+    <arg name="slam" default="true"/>
     <include if="$(arg slam)" file="$(find rtabmap_ros)/launch/slam.launch"/>
 
     <node pkg="lunabot_control" type="effort_factory.py" name="effort_factory" output="screen"/>
diff --git a/lunabot_bringup/launch/slam.launch b/lunabot_bringup/launch/slam.launch
index 0627a3e6..b5aeefc0 100644
--- a/lunabot_bringup/launch/slam.launch
+++ b/lunabot_bringup/launch/slam.launch
@@ -1,6 +1,5 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="slam" default="true"/>
     <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
         <arg name="rtabmap_args" value="--delete_db_on_start"/>
         <arg name="depth_topic" value="/d455_back/camera/aligned_depth_to_color/image_raw"/>

From 29a08e441a10f03ce8639d1b25a2e4cdc6abf55b Mon Sep 17 00:00:00 2001
From: nlaury <109548200+nlaury@users.noreply.github.com>
Date: Tue, 29 Oct 2024 22:53:08 -0400
Subject: [PATCH 5/5] fixed incorrect call to slam.launch

---
 lunabot_bringup/launch/autonomy.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lunabot_bringup/launch/autonomy.launch b/lunabot_bringup/launch/autonomy.launch
index c960c51e..1bbd1024 100644
--- a/lunabot_bringup/launch/autonomy.launch
+++ b/lunabot_bringup/launch/autonomy.launch
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="slam" default="true"/>
-    <include if="$(arg slam)" file="$(find rtabmap_ros)/launch/slam.launch"/>
+    <include if="$(arg slam)" file="$(find lunabot_bringup)/launch/slam.launch"/>
 
     <node pkg="lunabot_control" type="effort_factory.py" name="effort_factory" output="screen"/>