Added rotate acorns

toid_navigation/launch/main.py

@@ -1,7 +1,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.conditions import IfCondition, UnlessCondition 
-from launch.substitutions import  LaunchConfiguration
+from launch.substitutions import  LaunchConfiguration, AllSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -10,6 +10,7 @@ import os
 def generate_launch_description():
     nav_pkg_share = FindPackageShare("").find('toid_navigation')
     control_pkg_share = FindPackageShare("").find('toid_control')
+    lidar_pkg_share = FindPackageShare("").find('toid_lidar')
     params = os.path.join(nav_pkg_share, 'params', 'toid_general_params.yaml')
     map = os.path.join(nav_pkg_share, 'maps', 'mapb2_5cm.yaml')
     ctrl_launch_dir = os.path.join(control_pkg_share, 'launch')
@@ -37,6 +38,20 @@ def generate_launch_description():
         default_value='True',
         description="Whether to launch rviz2" 
     )
+
+    use_lidar = LaunchConfiguration("use_lidar")
+    use_lidar_arg = DeclareLaunchArgument(
+        'use_lidar',
+        default_value='True',
+        description="Whether to launch rviz2" 
+    )
+
+    is_blue = LaunchConfiguration("is_blue")
+    is_blue_arg = DeclareLaunchArgument(
+        'is_blue',
+        default_value='True',
+        description="Whether to launch rviz2" 
+    )
     
     toid_control = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -49,6 +64,17 @@ def generate_launch_description():
         condition=IfCondition(run_nodes),
     )
 
+    toid_lidar = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_pkg_share, 'launch' , 'launch.py')
+        ),
+        launch_arguments={
+            'visualize': 'False',
+            'lidar_frame': 'lidar_frame', 
+        }.items(),
+        condition=IfCondition(AllSubstitution(run_nodes, use_lidar)),
+    ) 
+
     map_server = Node(
         package='nav2_map_server',
         executable='map_server',
@@ -118,6 +144,8 @@ def generate_launch_description():
         visualize_arg,
         use_mock_arg,
         run_nodes_arg,
+        use_lidar_arg,
+        toid_lidar,
         rviz_node,
         map_server,
         bt_navigator,