Conversation
…unctionality and usage
- Introduced a comprehensive flow report detailing the system interaction diagram, node responsibilities, function-call flow, suggested refactor plan, and short-term fixes for the topological navigation system in ROS 2. - Included mermaid diagram for visual representation of system interactions. - Documented responsibilities and key files for each node involved in the navigation process. - Outlined a phased approach for refactoring to improve code structure and maintainability. - Suggested immediate fixes to enhance system reliability and consistency.
- Moved CustomSafeLoader to map_types.py for reuse across multiple scripts. - Introduced type-safe data classes (TopologicalPose, TopologicalEdge, TopologicalNode, TopologicalMap) to improve map handling. - Updated scripts to utilize the new CustomSafeLoader and data classes, enhancing code clarity and maintainability.
…pological_transform_publisher node
Topological NavigationA 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.
Table of Contents
Architecture OverviewData Flow:
Packagestopological_navigation (Core)
Key modules:
topological_navigation_msgs
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. topological_navigation_visual
Key executables:
topological_nav_simulator
Provides InstallationPrerequisites
Build from Source# 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.bashPython Dependencies
Launching the SystemFull Stack (with Simulator)Launch the complete topological navigation system with a virtual robot (no real hardware needed): ros2 launch topological_navigation topological_navigation.launch.pyThis 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.0Standalone SimulatorLaunch only the fake Nav2 server (useful when developing against a simulated robot): ros2 launch topological_nav_simulator fake_nav2.launch.pyLaunch arguments:
Visualiser OnlyLaunch the topological map visualiser standalone: ros2 launch topological_navigation_visual topological_map_visualiser.launch.py \
map_file:=/path/to/map.tmap2.yaml edit_mode:=trueLaunch arguments:
On a Real RobotWhen 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.pySending Navigation GoalsOnce 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}"ROS 2 Node Parameterstopological_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 TopicsPublished Topics
Subscribed Topics
TF Broadcasts
ROS 2 Services
ROS 2 ActionsAction Servers
Action Clients
TF FramesThe localisation node resolves the robot pose by looking up Custom Message, Service & Action DefinitionsAll definitions reside in the Messages (.msg)
Services (.srv)
Actions (.action)
Topological Map FormatTopological maps are YAML files with a 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.5Key sections:
Flexible Properties SystemBoth nodes and edges support an optional properties:
xy_goal_tolerance: 0.3 # Navigation tolerance
semantics: "row_entry" # Semantic label
roboflow: # Namespaced properties
enabled: true
confidence: 0.7Always 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. Map Validationros2 run topological_navigation validate_map.py /path/to/map.tmap2.yaml -vCommand-Line Utilities
Testing# 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"Contributing
LicenseApache License 2.0 — see LICENSE for details. |
The publishers (map_manager2, localisation2) use TRANSIENT_LOCAL durability. The test subscribers were using the default VOLATILE QoS, which caused a race condition in CI: if the subscriber connected after the one-time publish, it would never receive the message. Matching the QoS durability ensures late-joining subscribers get the last published value.
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 94 out of 280 changed files in this pull request and generated 14 comments.
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
You can also share your feedback on Copilot code review. Take the survey.
| setup( | ||
| name=package_name, | ||
| version='3.0.5', | ||
| version='5.0.0', # Major version bump - ROS1 code removed |
There was a problem hiding this comment.
The version comment and value indicate v5.0.0, while package.xml is v4.0.0 and the description text says ROS1 support was removed in v4.0.0. Pick a single versioning story (either bump package.xml to 5.0.0 or set setup.py back to 4.0.0) and ensure release notes/descriptions match the chosen version.
| version='5.0.0', # Major version bump - ROS1 code removed | |
| version='4.0.0', # Version aligned with package.xml (ROS1 code removed in v4.0.0) |
There was a problem hiding this comment.
We should have this consistent throughout, i'm not against being at v5... but we should have it matching all the way through. Verify what copilot picked up and see if it's valid.
There was a problem hiding this comment.
agree, it was mismatched in the original code. it is quite confusing as we do have the version number in the xml file and in setup.py file. I will make it consistent across both to v5
topological_navigation/README.md
Outdated
| - **[Interactive Map Editor](doc/INTERACTIVE_MAP_EDITOR.md)** - Edit maps visually in RViz2 | ||
| - **[Properties System](doc/PROPERTIES.md)** - Flexible node/edge metadata | ||
| - **[Quick Start](QUICK_START_INTERACTIVE_EDITOR.md)** - Get started with the interactive editor |
There was a problem hiding this comment.
doc/PROPERTIES.md is linked here but the PR deletes topological_navigation/doc/PROPERTIES.md, so this link will be broken. Either restore the doc (possibly updated for the new YAML-embedded definitions approach), or update the README to point to the new/renamed documentation location.
| ('share/' + package_name + '/config/', glob('config/*', recursive=True)), | ||
| ('share/' + package_name + '/launch/', glob('launch/*', recursive=True)) | ||
| ('share/' + package_name + '/config/', | ||
| glob('test/fixtures/mixed_actions_map.yaml')), |
There was a problem hiding this comment.
There are two data_files entries targeting the same destination directory (share/.../config/). While it can work, it’s easy to miss and can cause confusion during packaging/release reviews. Consider moving mixed_actions_map.yaml into config/ (so it’s covered by the existing glob), or merging all config-bound files into a single tuple for clarity.
|
|
||
| ## Validation | ||
|
|
||
| All fixtures should validate against `topological_navigation/config/schema2.json`. |
There was a problem hiding this comment.
This README references schema2.json, but the repo is using a YAML schema file (config/tmap-schema.yaml) in this PR, and it also includes absolute local paths under /home/... which won’t exist for other developers/CI. Update the schema references to the current schema file and replace/remove local absolute-path references (or link to repo-relative docs).
| - Requirements: `/home/ibrahim/.kiro/specs/localisation-networkx-refactor/requirements.md` | ||
| - Design: `/home/ibrahim/.kiro/specs/localisation-networkx-refactor/design.md` | ||
| - Schema: `topological_navigation/config/schema2.json` |
There was a problem hiding this comment.
This README references schema2.json, but the repo is using a YAML schema file (config/tmap-schema.yaml) in this PR, and it also includes absolute local paths under /home/... which won’t exist for other developers/CI. Update the schema references to the current schema file and replace/remove local absolute-path references (or link to repo-relative docs).
|
|
||
| import math | ||
|
|
||
| import rclpy |
There was a problem hiding this comment.
rclpy is imported but not used anywhere in this module. Removing unused imports reduces lint noise and makes dependencies clearer.
| import rclpy |
| from geometry_msgs.msg import Pose, Quaternion # noqa: E402 | ||
| from topological_nav_simulator.virtual_robot import ( | ||
| _euler_from_quaternion, | ||
| _lerp, |
There was a problem hiding this comment.
Pose and Quaternion are imported but not used in this test file. Consider removing them to keep the tests clean (and reduce the need for noqa exceptions).
| from geometry_msgs.msg import Pose, Quaternion # noqa: E402 | |
| from topological_nav_simulator.virtual_robot import ( | |
| _euler_from_quaternion, | |
| _lerp, | |
| from topological_nav_simulator.virtual_robot import ( | |
| _euler_from_quaternion, | |
| _lerp, | |
| _lerp, |
|
@Iranaphor can you please test if the execute policy works in the new implementations? |
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
There was a problem hiding this comment.
I think this needs further polishing before it can replace the state that the 'old' topo nav system was.
I'm not able to launch the specific toponav tools (i.e. adding points in rviz) Do we want to implement this alongside moving nodes (right click menu option to add node?)
Do we also want to be able to lock the nodes in place (ie for deployment)
I've left some comments throughout.
There was a problem hiding this comment.
Do we need this separate from copilot instructions? can they be symlinked?
| ┌──────────────────────────────────────────────┐ | ||
| │ fake_nav2_server (node) │ | ||
| │ │ | ||
| │ Action Servers: │ | ||
| │ /navigate_to_pose (NavigateToPose) │ | ||
| │ /navigate_through_poses(NavigateThroughPoses)│ | ||
| │ /follow_waypoints (FollowWaypoints) │ | ||
| │ │ | ||
| │ Publishers: │ | ||
| │ TF: map → odom → base_link │ | ||
| │ /odometry/global (Odometry) │ | ||
| │ /virtual_robot/markers (MarkerArray) │ | ||
| │ /virtual_robot/pose (PoseStamped) │ | ||
| │ │ | ||
| │ Subscribers: │ | ||
| │ /initialpose (teleport from RViz) │ | ||
| └──────────────────────────────────────────────┘ |
There was a problem hiding this comment.
Can this instead be a mermaid diagram so it is easier to read?
| # Copyright (c) 2026, topological_navigation contributors | ||
| # Licensed under the MIT License. |
There was a problem hiding this comment.
The overall repository is licensed as Apache-2.0 and Not MIT. Is this intentional?
topological_navigation/README.md
Outdated
| - **[Interactive Map Editor](doc/INTERACTIVE_MAP_EDITOR.md)** - Edit maps visually in RViz2 | ||
| - **[Properties System](doc/PROPERTIES.md)** - Flexible node/edge metadata | ||
| - **[Quick Start](QUICK_START_INTERACTIVE_EDITOR.md)** - Get started with the interactive editor |
| last_updated: | ||
| type: string | ||
| pattern: ^[0-9]{4}-[0-9]{2}-[0-9]{2}_[0-9]{2}-[0-9]{2}-[0-9]{2}$ | ||
| pattern: ^[0-9]{2}-[0-9]{2}-[0-9]{4}_[0-9]{2}-[0-9]{2}-[0-9]{2}$ |
There was a problem hiding this comment.
I think having both is a good option, but we should be following ISO8601 and historical versions and using yyyy-mm-dd
There was a problem hiding this comment.
I think this readme isn't as clear as it should be.
The TOC should be removed, as it just taking up vheight.
Is this something that should be crafted into a full documentation site (ala github wiki or read the docs etc)? Each TOC item should instead be a doc page?
TMap format should be a separate example file and or doc page.
There was a problem hiding this comment.
There's also unfinished sections (i.e. MongoDB) these should be completed or removed.
There was a problem hiding this comment.
Why are we using JSON Schemas instead of native ros2 types?
|
|
||
| setup( | ||
| name=package_name, | ||
| version='1.0.0', |
There was a problem hiding this comment.
Should this match the overarching packages version?
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>topological_nav_simulator</name> | ||
| <version>1.0.0</version> |
There was a problem hiding this comment.
this simulator is a new pkg that I added, so I think v1 is ok for it
| runs-on: ubuntu-latest | ||
| strategy: | ||
| matrix: | ||
| ros_distribution: |
There was a problem hiding this comment.
Should we also add Jazzy as a testing target?
There was a problem hiding this comment.
I think I have added it
There was a problem hiding this comment.
@cooperj, can you please add it and use aoc base images please
There was a problem hiding this comment.
Okay can do, working on this issue LCAS/aoc_container_base#5 before I can provide you a working version. Do you want me to add it to this PR?
There was a problem hiding this comment.
yes please to this pr as this one is already huge
There was a problem hiding this comment.
I think we need to review where this sits in terms of integration of the aoc_container_base. This means we would base this on lcas.lincoln.ac.uk/ros.
This would remove the need to create the lcas user here.
To then match other containers we should then have another target of devcontainer and then final.
The devcontainer then gives you a dev workspace and then final target gives you a finished deployable container.
This is assuming that our AOC use case is the most effective way of doing this for people outside of AOC.
Thoughts?
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
… ROS distribution setup
V5.0.0
Refactored the entire codebase by removing all ROS1 legacy components. Added spatial search capabilities and integrated NetworkX for graph-based search. Embedded behavior trees and actions directly within the map YAML file and reviewed the map configuration to remove unused parameters. Implemented a visualization package enabling RViz interaction with nodes, including direct drag-and-drop manipulation. Added a simulation package for testing navigation with Nav2.