Skip to content

Topological Navigation 2

Jump to bottom
Marc Hanheide edited this page Oct 15, 2026 · 2 revisions

Topological Navigation V2

This page describes the architecture of the ROS 2 implementation. For parameters, topics, services and actions see the repository README.

flowchart LR
    MM[map_manager2] -- /topological_map_2 --> LOC[localisation2]
    MM -- /topological_map_2 --> NAV[navigation2]
    LOC -- current_node, closest_node, closest_edges --> NAV
    NAV -- action goals --> NAV2[Nav2 or fake_nav2_server]
    MM -- /topological_map_2 --> VIS[map visualiser, web app]
    NAV -- Route, current_edge, status --> VIS
    VIS -- GotoNode goal --> NAV
Loading

Data flow

  1. Map manager (map_manager2.py) loads a .tmap2.yaml / .tmap3.yaml map, validates it against config/tmap-schema.yaml, publishes it as a latched std_msgs/String on /topological_map_2 and broadcasts a static TF from transformation.parent to transformation.topo_frame_id.
  2. Localisation (localisation2.py) builds a NetworkX graph with a KD-tree, looks up topo_frame_id -> base_frame in TF once per second and publishes /current_node (inside an influence zone, otherwise none), /closest_node, /closest_node_distance, /closest_edges and /current_node/tag.
  3. Navigation (navigation2.py) serves the GotoNode action on /topological_navigation. It plans a route (A* or Dijkstra), merges consecutive composable edges of the same action into segments, builds a goal for each segment from the map's action template and sends it to the matching action server (Nav2). Statistics are published and, if stats_db_path is set, stored in SQLite.
  4. Visualiser and web app render the map and route, and send GotoNode goals on click.

Navigation server state machine

navigation_graph.py provides NavState and NavStateMachine; navigation2.py waits for the map and for localisation, then moves to READY. The state is published as text on /robot_operation_current_status.

Map driven actions

Actions are not hard coded. The actions section of the map (or of a sidecar topological_navigation_config.yaml) defines, per action name, the ROS action type, the action server, whether consecutive edges can be composed into a single goal, and a goal template with ${node.*}, ${transformation.*} and ${definitions.*} placeholders. See Topological YAML Format.

Parameters of other ROS nodes (for example the Nav2 goal checker or collision monitor) can be changed for the duration of a segment with ros_parameters and set_ros_params; see doc/SET_ROS_PARAMS.md.

Source layout

topological_navigation/            core package
  topological_navigation/
    scripts/                       ROS nodes and CLI entry points
      map_manager2.py  localisation2.py  navigation2.py
      map_recorder.py  manual_topomapping.py  topo_stats.py
    navigation_graph.py            state machine, route planning, segment merging, boundary polygons
    networkx_utils.py              graph construction, KD-tree, point-in-polygon, localisation logic
    tmap_utils.py                  map loading (including sidecar config), saving
    map_recorder_utils.py          pure Python map building core
    map_analyser.py  map_merger.py validate_map.py  convert_tmap.py
    nav_stats_db.py  coverage_analysis.py  topomap_to_map_image.py
  config/                          schemas, node/edge/action templates, sample maps
  launch/  rviz/  doc/  test/
topological_navigation_msgs/       msg, srv, action
topological_navigation_visual/     visualiser, route visualiser, web app, policy markers
topological_nav_simulator/         fake Nav2 server and virtual robot

Clone this wiki locally