Adding new mission profile for row operation

topological_navigation/config/bt_tree_default.xml

@@ -6,7 +6,7 @@
           <RecoveryNode number_of_retries="0" name="ComputePathThroughPoses">
             <ReactiveSequence>
               <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="1.5"/>
-              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GoalAligned"/>
+              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
             </ReactiveSequence>
             <ReactiveFallback name="ComputePathThroughPosesRecoveryFallback">
               <GoalUpdated/>

topological_navigation/config/bt_tree_in_row_operation.xml

@@ -0,0 +1,10 @@
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="0" name="Harvesting">
+        <Wait wait_duration="5"/>
+        <ReactiveFallback name="ComputePathToPoseRecoveryFallback">
+        <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+        </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

topological_navigation/config/bt_tree_in_row_recovery.xml

@@ -0,0 +1,10 @@
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="0" name="Harvesting">
+        <Wait wait_duration="5"/>
+        <ReactiveFallback name="ComputePathToPoseRecoveryFallback">
+        <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+        </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

topological_navigation/config/template_edge.yaml

@@ -0,0 +1,15 @@
+action: move_base
+action_type: move_base_msgs/MoveBaseGoal
+config: []
+edge_id: origin_destination
+fail_policy: fail
+fluid_navigation: True
+goal:
+  target_pose:
+    header:
+      frame_id: $node.parent_frame
+    pose: $node.pose
+node: destination
+recovery_behaviours_config: ''
+restrictions_planning: 'True'
+restrictions_runtime: 'True'

topological_navigation/config/template_node_2.yaml

@@ -0,0 +1,41 @@
+meta:
+  map: map_2d
+  node: NodeName
+  pointset: PointSet
+node:
+  edges: []
+  localise_by_topic: ''
+  name: NodeName
+  parent_frame: map
+  pose:
+    orientation:
+      w: 1.0
+      x: 0.0
+      y: 0.0
+      z: 0.0
+    position:
+      x: 0.0
+      y: 0.0
+      z: 0.0
+  properties:
+    xy_goal_tolerance: 0.3
+    yaw_goal_tolerance: 6.29
+  restrictions_planning: 'True'
+  restrictions_runtime: 'True'
+  verts:
+  - x: 0.689999997616
+    y: 0.287000000477
+  - x: 0.287000000477
+    y: 0.490000009537
+  - x: -0.287000000477
+    y: 0.490000009537
+  - x: -0.689999997616
+    y: 0.287000000477
+  - x: -0.689999997616
+    y: -0.287000000477
+  - x: -0.287000000477
+    y: -0.490000009537
+  - x: 0.287000000477
+    y: -0.490000009537
+  - x: 0.689999997616
+    y: -0.287000000477

topological_navigation/launch/manual_topomapping.launch.py

@@ -0,0 +1,42 @@
+'''
+Author: Ibrahim Hroob <ihroob@lincoln.ac.uk> 2024
+'''
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        # Declare the launch arguments
+        DeclareLaunchArgument('tmap', default_value='test.yaml'),
+        DeclareLaunchArgument('tmap_dir', default_value='/home/ros/tmap'),
+        DeclareLaunchArgument('node_thresh', default_value='0.5'),
+        DeclareLaunchArgument('lock_btn', default_value='6'),
+        DeclareLaunchArgument('add_btn', default_value='1'),
+        DeclareLaunchArgument('remove_btn', default_value='2'),
+        DeclareLaunchArgument('gen_map_btn', default_value='3'),
+        DeclareLaunchArgument('topic_joy', default_value='/joy'),
+        DeclareLaunchArgument('topic_pose', default_value='/gps_base/odometry'),
+
+        # Launch the manual_topomapping node
+        Node(
+            package='topological_navigation',
+            executable='manual_topomapping.py',
+            name='manual_topomapping',
+            output='screen',
+            parameters=[{
+                'tmap'       : LaunchConfiguration('tmap'),
+                'tmap_dir'  : LaunchConfiguration('tmap_dir'),
+                'site_name'  : LaunchConfiguration('tmap'),
+                'node_thresh': LaunchConfiguration('node_thresh'),
+                'lock_btn'   : LaunchConfiguration('lock_btn'),
+                'add_btn'    : LaunchConfiguration('add_btn'),
+                'remove_btn' : LaunchConfiguration('remove_btn'),
+                'gen_map_btn': LaunchConfiguration('gen_map_btn'),
+                'topic_joy'  : LaunchConfiguration('topic_joy'),
+                'topic_pose' : LaunchConfiguration('topic_pose'),
+            }]
+        ),
+    ])

topological_navigation/setup.py

@@ -47,7 +47,8 @@
             'visualise_map_ros2.py = topological_navigation.scripts.visualise_map_ros2:main',
             'topomap_marker.py = topological_navigation.topomap_marker:main',
             'topomap_marker2.py = topological_navigation.topomap_marker2:main',
-            'policy_marker.py = topological_navigation.policy_marker:main'
+            'policy_marker.py = topological_navigation.policy_marker:main',
+            'manual_topomapping.py = topological_navigation.scripts.manual_topomapping:main'
         ],
     },