Created path segmenter controller

toid_navigation/launch/navigation_launch.py

@@ -13,6 +13,8 @@ def generate_launch_description():
     map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml')
     launch_dir = os.path.join(pkg_share, 'launch')
     default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'nav2.rviz')
+    bt_path = os.path.join(pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml")
+    lattice_path = os.path.join(pkg_share, "params", "output.json")
 
     position = "0.756 0.225 0 0 0 0".split(' ')
     #position = "0 0 0 0 0 0".split(' ')
@@ -54,7 +56,7 @@ def generate_launch_description():
         executable='planner_server',
         name='planner_server',
         output='screen',
-        parameters=[params]
+        parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params]
     )
 
     controller_server = Node(
@@ -70,9 +72,11 @@ def generate_launch_description():
         executable='bt_navigator',
         name='bt_navigator',
         output='screen',
-        parameters=[params]
+        parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params]
     )
 
+# 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',