Poop topo nodes (#187)

* Add manual population node for topological navigation nodes [ROS2] * adding launch file * Update test_navigationcore.py - increase test timeout --------- Co-authored-by: Marc Hanheide <[email protected]>
LCAS · Jan 16, 2025 · e4389ef · e4389ef
1 parent c6b40b6
commit e4389ef
Show file tree

Hide file tree

Showing 6 changed files with 516 additions and 4 deletions.
diff --git a/topological_navigation/config/template_edge.yaml b/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'
diff --git a/topological_navigation/config/template_node_2.yaml b/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
diff --git a/topological_navigation/launch/manual_topomapping.launch.py b/topological_navigation/launch/manual_topomapping.launch.py
@@ -0,0 +1,42 @@
+'''
+Author: Ibrahim Hroob <[email protected]> 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'),
+            }]
+        ),
+    ])
diff --git a/topological_navigation/setup.py b/topological_navigation/setup.py
@@ -48,7 +48,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'
         ],
     },
 

diff --git a/topological_navigation/test/test_navigationcore.py b/topological_navigation/test/test_navigationcore.py
@@ -58,7 +58,7 @@ def test_check_if_topological_map_is_published():
     try:
         node = NavigationClient('test_node')
         node.initialize()
-        msgs_received_flag = node.topmap_event_object.wait(timeout=20.0)
+        msgs_received_flag = node.topmap_event_object.wait(timeout=40.0)
         assert msgs_received_flag, 'Did not receive topological map !'
     finally:
         rclpy.shutdown()
@@ -69,7 +69,7 @@ def test_check_if_closest_node_is_published():
     try:
         node = NavigationClient('test_node')
         node.initialize()
-        msgs_received_flag = node.closest_node_event_object.wait(timeout=20.0)
+        msgs_received_flag = node.closest_node_event_object.wait(timeout=40.0)
         assert msgs_received_flag, 'Did not receive closest node info !'
     finally:
         rclpy.shutdown()
@@ -80,7 +80,7 @@ def test_check_if_current_node_is_published():
     try:
         node = NavigationClient('test_node')
         node.initialize()
-        msgs_received_flag = node.current_node_event_object.wait(timeout=20.0)
+        msgs_received_flag = node.current_node_event_object.wait(timeout=40.0)
         assert msgs_received_flag, 'Did not receive current node info !'
     finally:
         rclpy.shutdown()