From d16a9523b9ce7afa03b29468b78b5ac1e7e70237 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Mon, 2 Mar 2026 16:56:44 +0100
Subject: [PATCH] Added launch configuration for behavior server

---
 toid_navigation/launch/main.py      | 129 ++++++++++++++++++++++++++++
 toid_navigation/maps/mapb2_5cm.yaml |   4 +-
 2 files changed, 131 insertions(+), 2 deletions(-)
 create mode 100644 toid_navigation/launch/main.py

diff --git a/toid_navigation/launch/main.py b/toid_navigation/launch/main.py
new file mode 100644
index 0000000..9c02541
--- /dev/null
+++ b/toid_navigation/launch/main.py
@@ -0,0 +1,129 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition, UnlessCondition 
+from launch.substitutions import  LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+import os
+
+def generate_launch_description():
+    nav_pkg_share = FindPackageShare("").find('toid_navigation')
+    control_pkg_share = FindPackageShare("").find('toid_control')
+    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')
+    default_rviz_config_path = os.path.join(nav_pkg_share, 'rviz', 'nav2.rviz')
+    bt_path = os.path.join(nav_pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml")
+    lattice_path = os.path.join(nav_pkg_share, "params", "output.json")
+
+    visualize = LaunchConfiguration("visualize")
+    visualize_arg = DeclareLaunchArgument(
+        'visualize',
+        default_value='False',
+        description="Whether to launch rviz2" 
+    )
+
+    use_mock = LaunchConfiguration("use_mock")
+    use_mock_arg = DeclareLaunchArgument(
+        'use_mock',
+        default_value='True',
+        description="Whether to launch rviz2" 
+    )
+
+    run_nodes = LaunchConfiguration("run_nodes")
+    run_nodes_arg = DeclareLaunchArgument(
+        'run_nodes',
+        default_value='True',
+        description="Whether to launch rviz2" 
+    )
+    
+    toid_control = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(ctrl_launch_dir, 'toid.launch.py')
+        ),
+        launch_arguments={
+            'visualize': 'False',
+            'use_mock': use_mock
+        }.items(),
+        condition=IfCondition(run_nodes),
+    )
+
+    map_server = Node(
+        package='nav2_map_server',
+        executable='map_server',
+        name='map_server',
+        output='screen',
+        parameters=[{'yaml_filename': map}],
+        condition=IfCondition(run_nodes),
+    )
+
+    planner_server = Node(
+        package='nav2_planner',
+        executable='planner_server',
+        name='planner_server',
+        output='screen',
+        parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params],
+        condition=IfCondition(run_nodes),
+    )
+
+    controller_server = Node(
+        package='nav2_controller',
+        executable='controller_server',
+        name='controller_server',
+        output='screen',
+        parameters=[params],
+        condition=IfCondition(run_nodes),
+    )
+
+    bt_navigator = Node(
+        package='nav2_bt_navigator',
+        executable='bt_navigator',
+        name='bt_navigator',
+        output='screen',
+        parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params],
+        condition=IfCondition(run_nodes),
+    )
+
+# default_nav_to_pose_bt_xml: "$(find-pkg-share toid_navigation)/behaviors/navigate_to_pose_w_backtracking.xml"
+
+    behavior_server = Node(
+        package='nav2_behaviors',
+        executable='behavior_server',
+        name='behavior_server',
+        output='screen',
+        parameters=[params],
+        condition=IfCondition(run_nodes),
+    )
+
+    lifecycle_manager_node = Node(
+        package='nav2_lifecycle_manager',
+        executable='lifecycle_manager',
+        name='lifecycle_manager',
+        output='screen',
+        parameters=[params],
+        condition=IfCondition(run_nodes),
+    )
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', default_rviz_config_path],
+        condition=IfCondition(visualize)
+    )
+
+    return LaunchDescription([
+        visualize_arg,
+        use_mock_arg,
+        run_nodes_arg,
+        rviz_node,
+        map_server,
+        bt_navigator,
+        behavior_server,
+        planner_server,
+        controller_server,
+        lifecycle_manager_node,
+        toid_control
+    ])
\ No newline at end of file
diff --git a/toid_navigation/maps/mapb2_5cm.yaml b/toid_navigation/maps/mapb2_5cm.yaml
index ee47b45..e77f172 100644
--- a/toid_navigation/maps/mapb2_5cm.yaml
+++ b/toid_navigation/maps/mapb2_5cm.yaml
@@ -1,6 +1,6 @@
 image: mapb2_5cm.png
 resolution: 0.025
 origin: [-1.55, -1.05, 0.0]
-occupied_thresh: 0.10
-free_thresh: 0.06
+occupied_thresh: 0.89
+free_thresh: 0.88
 negate: 0
\ No newline at end of file