From b659cf20db2b18885063786a887c514031e41622 Mon Sep 17 00:00:00 2001
From: SimonR99 <simonroy99@hotmail.ca>
Date: Thu, 9 May 2024 21:33:35 -0400
Subject: [PATCH] create sim and real + readme

---
 README.md                                  |  2 +-
 src/rove_bringup/launch/autonomy.launch.py | 11 +++----
 src/rove_bringup/launch/common.launch.py   | 13 +++++++++
 src/rove_bringup/launch/real.launch.py     | 34 ++++++++++++++++++++++
 src/rove_bringup/launch/sim.launch.py      |  8 -----
 5 files changed, 54 insertions(+), 14 deletions(-)
 create mode 100644 src/rove_bringup/launch/real.launch.py

diff --git a/README.md b/README.md
index 2f7509d..a3b8dc9 100644
--- a/README.md
+++ b/README.md
@@ -58,7 +58,7 @@ Do these commands to run the gazebo simulation with physics enabled
 ```bash
 colcon build --symlink-install
 source install/setup.bash
-ros2 launch rove_description sim.launch.py
+ros2 launch rove_bringup sim.launch.py
 ```
 OR Do these commands to only run the rviz simulation (with joints control)
 ```bash
diff --git a/src/rove_bringup/launch/autonomy.launch.py b/src/rove_bringup/launch/autonomy.launch.py
index f6d1a95..c1d2ba2 100644
--- a/src/rove_bringup/launch/autonomy.launch.py
+++ b/src/rove_bringup/launch/autonomy.launch.py
@@ -5,10 +5,8 @@
 from launch.actions import IncludeLaunchDescription
 
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import Command
-from launch_ros.actions import Node
-from launch_ros.parameter_descriptions import ParameterValue
-from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition
 
 
 def generate_launch_description():
@@ -17,6 +15,8 @@ def generate_launch_description():
     pkg_rove_nav = get_package_share_directory('rove_navigation')
     slam_pkg_path = get_package_share_directory("slam_toolbox")  
 
+    use_slam3d = LaunchConfiguration('use_slam3d')
+
     slam = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(slam_pkg_path, "launch", "online_async_launch.py"),
@@ -37,6 +37,7 @@ def generate_launch_description():
             "use_sim_time": "true",
             "deskewing": "true",
         }.items(),
+        condition=IfCondition(use_slam3d)
     )
 
     nav = IncludeLaunchDescription(
@@ -48,6 +49,6 @@ def generate_launch_description():
 
     return LaunchDescription([
             slam,
-            #slam3d,
+            slam3d,
             nav,
             ])
diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py
index 3772bcd..b88bcd7 100644
--- a/src/rove_bringup/launch/common.launch.py
+++ b/src/rove_bringup/launch/common.launch.py
@@ -12,6 +12,7 @@
 
 def generate_launch_description():
     # Get the launch directory
+    pkg_rove_bringup = get_package_share_directory('rove_bringup')
     pkg_rove_description = get_package_share_directory('rove_description')
     pkg_rove_slam = get_package_share_directory('rove_slam')
     bringup_pkg_path = get_package_share_directory('rove_bringup')
@@ -86,6 +87,17 @@ def generate_launch_description():
         ),
     )
 
+    autonomy = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "autonomy.launch.py"),
+        ),
+        launch_arguments={
+            "use_sim_time": "true",
+            "deskewing": "true",
+            "use_slam3d": "false",
+        }.items(),
+    )
+
     return LaunchDescription([
             robot_state_publisher,
             robot_localization_node_local,
@@ -93,4 +105,5 @@ def generate_launch_description():
             navsat_transform,
             rviz,
             teleop,
+            autonomy,
             ])
diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py
new file mode 100644
index 0000000..ef14e0b
--- /dev/null
+++ b/src/rove_bringup/launch/real.launch.py
@@ -0,0 +1,34 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    # Get the launch directory
+    pkg_rove_bringup = get_package_share_directory('rove_bringup')
+
+    common = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "common.launch.py"),
+        ),
+    )
+
+
+    ###### Sensor ######
+    vectornav = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "vectornav.launch.py"),
+        ),
+    )
+
+    return LaunchDescription([
+            common,
+            vectornav,
+            ])
diff --git a/src/rove_bringup/launch/sim.launch.py b/src/rove_bringup/launch/sim.launch.py
index 6e434e9..8b5e118 100644
--- a/src/rove_bringup/launch/sim.launch.py
+++ b/src/rove_bringup/launch/sim.launch.py
@@ -74,18 +74,10 @@ def generate_launch_description():
         ),
     )
 
-
-    autonomy = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_rove_bringup, "launch", "autonomy.launch.py"),
-        ),
-    )
-
     return LaunchDescription([
             gz_sim,
             bridge,
             robot_state_publisher,
             create,
             common,
-            autonomy,
             ])