Hodgepodge of fixes

toid_navigation/launch/main.py

@@ -11,6 +11,7 @@ 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')
+    vision_pkg_share = FindPackageShare("").find('toid_vision')
     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')
@@ -70,11 +71,29 @@ def generate_launch_description():
         ),
         launch_arguments={
             'visualize': 'False',
-            'lidar_frame': 'lidar_frame', 
+            'lidar_frame': 'lidar', 
         }.items(),
         condition=IfCondition(AllSubstitution(run_nodes, use_lidar)),
     ) 
 
+    toid_vision = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(vision_pkg_share, 'launch' , 'launch.py')
+        ),
+        launch_arguments={
+            'is_blue': is_blue,
+        }.items(),
+        condition=IfCondition(AllSubstitution(run_nodes, use_lidar)),
+    ) 
+
+    toid_interaction = Node(
+        package='toid_interaction',
+        executable='node',
+        name='toid_interaction',
+        output='screen',
+        condition=IfCondition(run_nodes),
+    )
+
     map_server = Node(
         package='nav2_map_server',
         executable='map_server',
@@ -145,7 +164,10 @@ def generate_launch_description():
         use_mock_arg,
         run_nodes_arg,
         use_lidar_arg,
+        is_blue_arg,
         toid_lidar,
+        toid_vision,
+        toid_interaction,
         rviz_node,
         map_server,
         bt_navigator,