Topological Navigation

A ROS 2 framework for graph-based topological navigation of autonomous mobile robots. Unlike traditional metric navigation that operates in continuous coordinate space, topological navigation represents the environment as a graph of discrete nodes (waypoints) connected by edges (traversable paths), enabling high-level route planning, named-location navigation, and pluggable edge actions (e.g., Nav2 goals, row traversal, door opening).

Originally developed for the STRANDS long-term autonomy project, this framework is actively used in agricultural robotics for autonomous navigation in vineyards, orchards, and crop rows.

ROS 2 Only — version 4.0.0+ is ROS 2 exclusive (Humble / Iron). For ROS 1 support use version 3.x or earlier.

• Topological Navigation
  • Table of Contents
  • Architecture Overview
  • Packages
    • topological_navigation (Core)
    • topological_navigation_msgs
    • topological_navigation_visual
    • topological_nav_simulator
  • Installation
    • Prerequisites
    • Build from Source
    • Python Dependencies
  • Launching the System
    • Full Stack (with Simulator)
    • Standalone Simulator
    • Visualiser Only
    • On a Real Robot
    • Sending Navigation Goals
  • ROS 2 Node Parameters
    • topological_navigation (navigation2.py)
    • topological_localisation (localisation2.py)
    • topological_map_manager_2 (map_manager2.py)
    • manual_tmapping_node (manual_topomapping.py)
    • topological_map_visualiser (topological_map_visualiser.py)
    • route_visualiser / occupancy_visualiser (topological_visual.py)
    • fake_nav2_server (fake_nav2_server.py — simulator)
  • ROS 2 Topics
    • Published Topics
    • Subscribed Topics
    • TF Broadcasts
  • ROS 2 Services
  • ROS 2 Actions
    • Action Servers
    • Action Clients
  • TF Frames
  • Custom Message, Service & Action Definitions
    • Messages (.msg)
    • Services (.srv)
    • Actions (.action)
  • Topological Map Format
    • Flexible Properties System
    • Map Validation
  • Command-Line Utilities
  • Testing
  • Contributing
  • License

┌──────────────────────────────────────────────────────────────────────┐
│                        Topological Navigation Stack                  │
│                                                                      │
│  ┌────────────────┐   ┌──────────────────┐   ┌───────────────────┐  │
│  │  Map Manager   │──▶│  Localisation     │──▶│  Navigation       │  │
│  │  (map_manager2)│   │  (localisation2)  │   │  (navigation2)    │  │
│  │                │   │                   │   │                   │  │
│  │ Loads YAML map │   │ Determines        │   │ A* route planning │  │
│  │ Publishes to   │   │ current/closest   │   │ Edge action exec  │  │
│  │ /topological_  │   │ node via KD-tree  │   │ GotoNode action   │  │
│  │ map_2 topic    │   │ + influence zones │   │ server            │  │
│  └────────────────┘   └──────────────────┘   └────────┬──────────┘  │
│                                                        │             │
│                                                        ▼             │
│                                              ┌───────────────────┐  │
│                                              │  Nav2 Stack       │  │
│                                              │  (or Simulator)   │  │
│                                              │  NavigateToPose   │  │
│                                              │  NavigateThrough  │  │
│                                              │  FollowWaypoints  │  │
│                                              └───────────────────┘  │
│                                                                      │
│  ┌────────────────────┐   ┌──────────────────────────────────────┐  │
│  │  Map Visualiser    │   │  Fake Nav2 Simulator                 │  │
│  │  (visual package)  │   │  (simulator package)                 │  │
│  │  RViz markers +    │   │  Virtual robot + fake action servers │  │
│  │  interactive edit  │   │  for testing without real hardware   │  │
│  └────────────────────┘   └──────────────────────────────────────┘  │
└──────────────────────────────────────────────────────────────────────┘

Data Flow:

1. Map Manager loads a .tmap2.yaml file and publishes it as a JSON string on /topological_map_2
2. Localisation subscribes to the map, builds a NetworkX graph with KD-tree spatial indexing, and continuously publishes which node the robot is at (/current_node, /closest_node)
3. Navigation receives GotoNode action goals, computes A* routes, and executes edge actions by calling Nav2 action servers
4. Visualiser renders the topological map as interactive RViz markers for viewing and editing

Key modules:

Defines 16 message types, 9 service types, and 3 action types used across the navigation stack. See Custom Message, Service & Action Definitions below for full details.

Key executables:

Provides NavigateToPose, NavigateThroughPoses, and FollowWaypoints action servers that simulate robot movement, publishing TF transforms, odometry, and RViz markers. The simulated robot smoothly interpolates toward goal poses at a configurable speed.

• ROS 2 Humble or Iron
• Python 3.8+
• Nav2 (for real robot deployment; the simulator replaces it for testing)

# Create workspace
mkdir -p ~/ws/src && cd ~/ws/src
git clone https://github.com/LCAS/topological_navigation.git

# Install dependencies
cd ~/ws
rosdep install --from-paths src --ignore-src -r -y

# Build
colcon build --symlink-install

# Source
source install/setup.bash

Launch the complete topological navigation system with a virtual robot (no real hardware needed):

ros2 launch topological_navigation topological_navigation.launch.py

This starts (in order with delays for dependency readiness):

Launch arguments:

Example with a custom map:

ros2 launch topological_navigation topological_navigation.launch.py \
    map_path:=/path/to/my_map.tmap2.yaml \
    initial_x:=5.0 initial_y:=3.0

Launch only the fake Nav2 server (useful when developing against a simulated robot):

ros2 launch topological_nav_simulator fake_nav2.launch.py

Launch arguments:

Launch the topological map visualiser standalone:

ros2 launch topological_navigation_visual topological_map_visualiser.launch.py \
    map_file:=/path/to/map.tmap2.yaml edit_mode:=true

Launch arguments:

When using a real robot with Nav2, launch the navigation stack without the simulator:

# 1. Start Nav2 (robot-specific)
ros2 launch <your_robot_pkg> navigation.launch.py

# 2. Start the map manager
ros2 run topological_navigation map_manager2.py /path/to/map.tmap2.yaml

# 3. Start localisation
ros2 run topological_navigation localisation2.py

# 4. Start topological navigation
ros2 run topological_navigation navigation2.py

# 5. (Optional) Start visualiser
ros2 run topological_navigation_visual topological_map_visualiser.py

Once the system is running, send a robot to a named node:

# Navigate to a node called "node3"
ros2 action send_goal /topological_navigation \
    topological_navigation_msgs/action/GotoNode \
    "{target: 'node3', no_orientation: false}"

map (global frame)
 ├── topo_frame_id (static TF, published by map_manager2)
 │    └── Topological node poses are defined in this frame
 │
 └── odom (dynamic TF, published by simulator or real robot)
      └── base_link (robot body frame)

The localisation node resolves the robot pose by looking up topo_frame_id → base_frame via tf2_ros.

All definitions reside in the topological_navigation_msgs package.

Topological maps are YAML files with a .tmap2.yaml extension. Here is an abbreviated example:

meta:
  last_updated: 08-03-2026_12-00-00
  origin:
    latitude: 53.268
    longitude: -0.524
    altitude: 0

metric_map: my_farm
name: my_farm
pointset: my_farm

transformation:
  topo_frame_id: my_farm      # Child frame for node poses
  parent: map                  # Parent TF frame
  rotation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}
  translation: {x: 0.0, y: 0.0, z: 0.0}

definitions:
  default_bt: |
    <root main_tree_to_execute="MainTree">
      <BehaviorTree ID="MainTree">
        <PipelineSequence name="NavigateWithReplanning">
          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
          <FollowPath path="{path}" controller_id="FollowPath"/>
        </PipelineSequence>
      </BehaviorTree>
    </root>

actions:
  navigate_to_pose:
    composable: false
    action_type: nav2_msgs.action.NavigateToPose
    action_server: /navigate_to_pose
    action_goal_template:
      pose:
        header:
          frame_id: '${node.nav_frame}'
        pose: '${node.pose}'
      behavior_tree: '${definitions.default_bt}'

nodes:
  - meta:
      map: my_farm
      node: RowEntry_A1
      pointset: my_farm
    node:
      name: RowEntry_A1
      nav_frame: '${transformation.topo_frame_id}'
      pose:
        position: {x: 10.5, y: 5.2, z: 0.0}
        orientation: {w: 0.707, x: 0.0, y: 0.0, z: 0.707}
      properties:                    # Flexible key-value metadata
        xy_goal_tolerance: 0.3
        yaw_goal_tolerance: 0.1
      verts:                         # Influence zone polygon
        - {x: -0.5, y: -0.5}
        - {x:  0.5, y: -0.5}
        - {x:  0.5, y:  0.5}
        - {x: -0.5, y:  0.5}
      edges:
        - edge_id: RowEntry_A1_RowEnd_A1
          node: RowEnd_A1            # Target node name
          action: navigate_to_pose   # Action key from 'actions' section
          properties:
            max_speed: 0.5

Key sections:

• meta: Map metadata (timestamps, origin coordinates)
• transformation: TF frame for all node poses (published as a static transform)
• definitions: Reusable templates (e.g., Nav2 behavior trees)
• actions: Named action configurations — each maps to a ROS 2 action server with a goal template supporting variable substitution (${node.pose}, ${definitions.default_bt}, etc.)
• nodes: List of topological nodes, each with pose, properties, influence zone vertices, and outgoing edges

Both nodes and edges support an optional properties dictionary. Properties are freeform key-value pairs — no schema changes needed:

properties:
  xy_goal_tolerance: 0.3          # Navigation tolerance
  semantics: "row_entry"          # Semantic label
  roboflow:                       # Namespaced properties
    enabled: true
    confidence: 0.7

Always access properties defensively in code:

props = node["node"].get("properties", {})
tolerance = props.get("xy_goal_tolerance", 0.5)

See doc/PROPERTIES.md for full documentation.

ros2 run topological_navigation validate_map.py /path/to/map.tmap2.yaml -v

# Build the workspace
colcon build

# Run all tests for the core package
colcon test --packages-select topological_navigation --event-handlers console_direct+

# Run specific tests
cd topological_navigation
pytest test/test_networkx_utils.py -v
pytest test/test_localisation2.py -v

# Python linting
colcon test --packages-select topological_navigation \
    --event-handlers console_direct+ --pytest-args -k "test_flake8 or test_pep257"

1. Follow PEP 8 / PEP 257 for Python code
2. Use ament_flake8 and ament_pep257 for validation
3. Add tests for new functionality
4. Validate maps with validate_map.py
5. Update documentation when changing behavior

Apache License 2.0 — see LICENSE for details.