diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md new file mode 100644 index 00000000..48ed6e5f --- /dev/null +++ b/.github/copilot-instructions.md @@ -0,0 +1,174 @@ +# GitHub Copilot Instructions for topological_navigation + +This repository contains a ROS 2 topological navigation framework for autonomous mobile robots. + +## Repository Overview + +This is a multi-package ROS 2 workspace with the following packages: + +- **topological_navigation**: Core navigation and mapping functionality (Python) +- **topological_navigation_msgs**: Message, service, and action definitions +- **topological_rviz_tools**: RViz-based interactive map construction tools (C++) +- **topological_utils**: Command-line utilities for map creation and management (Python) + +## Code Style and Conventions + +### Python Code +- **Style Guide**: Follow PEP 8 and PEP 257 for Python code +- **Linters**: Use `ament_flake8` and `ament_pep257` for validation +- **Testing**: Use `pytest` and `launch_pytest` for tests +- **ROS 2 Patterns**: Follow standard ROS 2 node patterns with lifecycle management +- **Type Hints**: Use type hints where appropriate for better code clarity + +### C++ Code +- **Style Guide**: Follow ROS 2 C++ style guidelines +- **Build System**: Uses `ament_cmake` for C++ packages +- **RViz Plugins**: Follow RViz plugin development patterns for tool and panel creation + +### File Organization +- Scripts go in `{package}/scripts/` directory +- Python modules in `{package}/{package}/` directory +- Launch files in `{package}/launch/` directory +- Configuration files (YAML schemas, templates) in `{package}/config/` directory +- Tests in `{package}/test/` or `{package}/tests/` directories + +## Key Concepts + +### Topological Maps +- Maps are stored as YAML files (`.tmap2.yaml` extension) +- Schema validation is available via `validate_map.py` +- Maps consist of **nodes** (waypoints) and **edges** (connections) +- Each node has a pose, properties, and edges to other nodes +- Each edge has an action (e.g., NavigateToPose), action type, and optional properties + +### Flexible Properties System +- Both nodes and edges support a flexible `properties` dictionary +- Properties can be namespaced for organization (e.g., `restrictions`, `semantics`, `navigation`) +- Properties are optional and domain-specific +- See `topological_navigation/doc/PROPERTIES.md` for detailed documentation + +### Core Components +- **Map Manager**: Manages topological map data and publishes map information +- **Localisation**: Determines robot's current topological node +- **Navigation**: Plans and executes routes through the topological map +- **Route Search**: A* based path planning on topological graphs +- **Edge Actions**: Pluggable actions for traversing edges (e.g., NavigateToPose) + +## Building and Testing + +### Build the Workspace +```bash +colcon build +``` + +### Run Linters +```bash +# Python linting +colcon test --packages-select topological_navigation --event-handlers console_direct+ --pytest-args -k "test_flake8 or test_pep257" + +# Run all tests for a package +colcon test --packages-select topological_navigation +``` + +### Run Specific Tests +```bash +# Run pytest tests directly +cd topological_navigation +pytest test/test_navigationcore.py -v +``` + +### Validate Topological Maps +```bash +ros2 run topological_navigation validate_map.py path/to/map.tmap2.yaml -v +``` + +## Common Tasks + +### Adding New Node Properties +1. Update documentation in `topological_navigation/doc/PROPERTIES.md` with new property descriptions +2. Access properties safely in code using `.get()` with defaults +3. Consider using namespaces to organize properties by domain or package + +### Creating New Scripts +1. Add Python script to `{package}/scripts/` +2. Update `entry_points` in `setup.py` to expose as ROS 2 executable +3. Follow ROS 2 node patterns: use `rclpy.init()`, create node class, spin +4. Add appropriate command-line argument parsing + +### Modifying Map Schema +1. Update schema in `topological_navigation/config/tmap-schema.yaml` +2. Update validation logic if needed +3. Update templates in `config/template_node_2.yaml` and `config/template_edge.yaml` +4. Update documentation in `doc/PROPERTIES.md` + +### Working with RViz Tools (C++) +1. RViz tools inherit from `rviz_common::Tool` +2. RViz panels inherit from `rviz_common::Panel` +3. Use Qt for UI components in panels +4. Register plugins in `plugin_description.xml` + +## Important Files + +### Configuration +- `config/tmap-schema.yaml`: JSON schema for topological map validation +- `config/template_node_2.yaml`: Template for creating new nodes +- `config/template_edge.yaml`: Template for creating new edges + +### Documentation +- `README.md`: Package-level documentation +- `doc/PROPERTIES.md`: Comprehensive guide to the properties system +- `CHANGELOG.rst`: Version history and changes + +### Testing +- `test/`: Unit tests and integration tests +- Uses `pytest` for Python tests +- Uses `launch_pytest` for ROS 2 launch-based tests + +## CI/CD + +- GitHub Actions workflows in `.github/workflows/` +- `ci.yaml`: Builds and tests on ROS 2 Humble and Iron distributions +- `validate-schema.yaml`: Validates topological map schema +- All PRs should pass CI checks before merging + +## Dependencies + +### ROS 2 Dependencies +- `nav2_msgs`: Navigation 2 message definitions +- `geometry_msgs`, `std_msgs`, `nav_msgs`, `sensor_msgs`: Standard ROS 2 messages +- `tf_transformations`: Coordinate transformations +- `visualization_msgs`: RViz markers and visualization + +### Python Dependencies +- Standard library modules are preferred +- Minimal external dependencies to maintain portability + +## Best Practices + +### When Adding Features +1. Maintain backward compatibility with existing topological maps +2. Make properties optional with sensible defaults +3. Document new properties or features in relevant documentation files +4. Add validation logic to catch misconfigurations early +5. Add tests for new functionality + +### When Modifying Core Logic +1. Understand the impact on navigation behavior +2. Test with sample topological maps +3. Consider edge cases (empty maps, disconnected graphs, invalid actions) +4. Update documentation if behavior changes + +### When Working with Maps +1. Always validate maps with `validate_map.py` before deployment +2. Use namespaced properties to avoid conflicts between applications +3. Access properties defensively with `.get()` and default values +4. Document custom properties in your application-specific docs + +## Tips for AI Assistants + +- This is a ROS 2 package (not ROS 1) - use ROS 2 APIs and patterns +- Topological navigation differs from metric navigation: it operates on graphs of nodes, not continuous space +- Properties are flexible but optional - don't assume they exist +- Many scripts are command-line tools - respect existing argument parsing patterns +- Tests should be added for new functionality, following existing test patterns +- The repository supports multiple ROS 2 distributions (Humble, Iron) - use compatible APIs diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index e2504dc6..078a646f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -2,9 +2,11 @@ name: ros CI on: push: - branches: [ humble-dev ] + branches: [ humble-dev, aoc ] + tags: # Also run CI on tags. + - '*' pull_request: - branches: [ humble-dev ] + branches: [ humble-dev, aoc ] jobs: test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. diff --git a/.github/workflows/validate-schema.yaml b/.github/workflows/validate-schema.yaml new file mode 100644 index 00000000..d4642f21 --- /dev/null +++ b/.github/workflows/validate-schema.yaml @@ -0,0 +1,51 @@ +name: Validate Schema + +on: + push: + branches: [ humble-dev, aoc ] + tags: + - '*' + pull_request: + branches: [ humble-dev, aoc ] + +jobs: + validate_schema: + name: Validate YAML Schema + runs-on: ubuntu-latest + steps: + - name: Checkout code + uses: actions/checkout@v4 + + - name: Set up Python + uses: actions/setup-python@v6 + with: + python-version: '3.12' + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install jsonschema pyyaml check-jsonschema + + - name: Validate schema file + run: | + # Validate that the schema itself is valid JSON Schema Draft 7 + check-jsonschema --check-metaschema topological_navigation/config/tmap-schema.yaml + + - name: Test schema with Python + run: | + python -c " + import yaml + import jsonschema + + # Load the schema + with open('topological_navigation/config/tmap-schema.yaml', 'r') as f: + schema = yaml.safe_load(f) + + # Verify it's a valid JSON Schema by creating a validator + try: + jsonschema.validators.Draft7Validator.check_schema(schema) + print('✓ Schema is valid JSON Schema Draft 7') + except jsonschema.exceptions.SchemaError as e: + print(f'✗ Schema validation failed: {e}') + exit(1) + " diff --git a/.gitignore b/.gitignore index 89525881..70e59f66 100644 --- a/.gitignore +++ b/.gitignore @@ -67,3 +67,4 @@ coverage.xml # Nano auto-backups **/*.*~ **/*.*.swp +.DS_Store diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 00000000..3e001aca --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,812 @@ +# AI Coding Agent Guide for topological_navigation + +This document provides comprehensive information about the topological_navigation repository to help AI coding agents understand and work effectively with the codebase. + +## Project Overview + +**topological_navigation** is a ROS 2 framework for 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 (paths). + +### Purpose +This system enables robots to: +- Navigate between named locations using high-level graph-based planning +- Represent complex environments efficiently as topological maps +- Execute domain-specific actions when traversing edges (e.g., opening doors, climbing ramps, row operations in agriculture) +- Leverage flexible metadata for application-specific customization +- Support agricultural robot operations including in-row navigation and boundary detection + +### Originally Developed For +STRANDS (Spatio-Temporal Representations and Activities for Cognitive Control in Long-term Scenarios) - a long-term autonomy project for mobile robots. Currently actively used in agricultural robotics applications, particularly for autonomous navigation in vineyards, orchards, and crop rows. + +### Recent Development Focus (aoc branch) +The `aoc` branch focuses on: +- Enhanced edge action management for agricultural operations +- Row operation handling with boundary node selection +- Improved closest node detection algorithms +- AI agent preparation with comprehensive documentation +- Flexible node/edge property system for domain-specific metadata + +## Repository Structure + +This is a multi-package ROS 2 workspace with four packages: + +``` +topological_navigation/ +├── topological_navigation/ # Core package (Python) +│ ├── topological_navigation/ # Python modules +│ │ ├── scripts/ # ROS 2 executable scripts +│ │ │ ├── navigation2.py # Main navigation action server +│ │ │ ├── map_manager.py # Map loading and publishing +│ │ │ ├── localisation2.py # Topological localisation +│ │ │ ├── map_publisher.py # Map topic publisher +│ │ │ └── actions_bt.py # Behaviour tree action types +│ │ ├── edge_action_manager2.py # Edge action execution (67KB, complex) +│ │ ├── manager2.py # Map management core (58KB) +│ │ ├── route_search2.py # A* path planning +│ │ ├── topological_map.py # Map data structures +│ │ ├── load_maps_from_yaml.py # YAML map loader +│ │ └── restrictions_impl.py # Navigation restrictions +│ ├── config/ # YAML schemas and templates +│ │ └── schema2.json # JSON schema for map validation +│ ├── doc/ # Documentation +│ │ └── PROPERTIES.md # Properties system guide +│ ├── launch/ # ROS 2 launch files +│ └── test/, tests/ # Unit and integration tests +│ +├── topological_navigation_msgs/ # Message definitions +│ ├── msg/ # ROS 2 messages +│ ├── srv/ # ROS 2 services +│ └── action/ # ROS 2 actions +│ +├── topological_rviz_tools/ # Interactive map editing (C++/Qt) +│ ├── src/ # RViz plugins (tools and panels) +│ ├── include/ # C++ headers +│ └── scripts/ # Python interface +│ +└── topological_utils/ # Utilities (Python) + └── topological_utils/ # Map creation and management tools +``` + +## Technology Stack + +- **Language**: Python 3 (core navigation), C++ (RViz tools) +- **Framework**: ROS 2 (Humble, Iron distributions) +- **Build System**: ament_python, ament_cmake +- **Testing**: pytest, launch_pytest, ament linters +- **GUI**: Qt (for RViz panels) +- **Data Format**: YAML for topological maps (`.tmap2.yaml` extension) +- **Validation**: JSON Schema (`config/schema2.json`) +- **Key Dependencies**: + - `nav2_msgs` - Nav2 navigation actions and messages + - `geometry_msgs`, `nav_msgs`, `sensor_msgs` - ROS 2 standard messages + - `tf_transformations` - Coordinate frame transformations + - `visualization_msgs` - RViz markers + - `topological_navigation_msgs` - Custom message definitions + +## Core Concepts + +### 1. Topological Maps + +Topological maps are the fundamental data structure in this system. + +**Structure**: +- **Nodes**: Represent discrete locations (waypoints) with: + - Unique name (string identifier) + - Pose (position + orientation) in a coordinate frame + - Parent frame (typically "map") + - Influence zone (polygon defining the node's spatial extent) + - Flexible properties dictionary (optional) + - List of outgoing edges + +- **Edges**: Represent navigable connections with: + - Target node reference (node name) + - Action name (e.g., "NavigateToPose", "OpenDoor", "RowOperation") + - Action type (ROS 2 message/action type) + - Edge ID (unique identifier) + - Flexible properties dictionary (optional) + +**File Format**: YAML with `.tmap2.yaml` extension + +**Example Node**: +```yaml +nodes: + - meta: + map: vineyard_01 + node: RowEntry_A1 + pointset: vineyard_01 + node: + name: RowEntry_A1 + parent_frame: map + 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: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: "row_entry" + roboflow: + enabled: true + confidence: 0.7 + edges: + - edge_id: RowEntry_A1_RowEnd_A1 + node: RowEnd_A1 + action: RowOperation + action_type: nav2_msgs/action/NavigateToPose + properties: + max_speed: 0.5 + row_type: "vineyard" +``` + +### 2. Flexible Properties System + +Both nodes and edges support an optional `properties` dictionary for application-specific metadata. + +**Key Features**: +- Completely flexible schema (any key-value pairs) +- Optional - properties can be omitted entirely +- Supports namespacing for organization +- Enables domain-specific customization without schema changes +- Validated against JSON schema for structural correctness + +**Common Property Patterns**: +- **Flat structure**: `{capacity: 2, zone: "A"}` +- **Domain namespaces**: `{restrictions: {capacity: 2}, semantics: {zone: "A"}}` +- **Package namespaces**: `{roboflow: {enabled: true, confidence: 0.7}}` +- **Navigation parameters**: `{xy_goal_tolerance: 0.3, max_speed: 0.8}` + +**Important**: Always access properties defensively: +```python +# Safe property access pattern +props = node["node"].get("properties", {}) +capacity = props.get("capacity", 1) # Default to 1 + +# Nested property access +roboflow_config = props.get("roboflow", {}) +enabled = roboflow_config.get("enabled", False) +``` + +See `topological_navigation/doc/PROPERTIES.md` for comprehensive documentation. + +### 3. Navigation Architecture + +**Key Components**: + +1. **Map Manager** (`scripts/map_manager.py`, `manager2.py`) + - Loads topological maps from YAML files + - Publishes map structure to ROS 2 topics (`/topological_map_2`) + - Handles map updates and modifications + - Validates map structure against JSON schema + - Manages node and edge metadata + +2. **Localisation** (`scripts/localisation2.py`) + - Determines robot's current topological node + - Publishes `/current_node` and `/closest_node` + - Supports pose-based and topic-based localization + - Uses influence zones to determine node proximity + - Critical for navigation start conditions + +3. **Navigation** (`scripts/navigation2.py`) + - Executes topological navigation actions + - Coordinates with metric navigation (Nav2) + - Handles edge action execution via EdgeActionManager + - Implements navigation state machine + - Provides `/topological_navigation` action server + +4. **Route Search** (`route_search2.py`) + - A* based path planning on topological graph + - Finds optimal routes between nodes + - Considers edge properties for path optimization + - Handles node restrictions and blocked edges + - Returns ordered list of nodes and edges to traverse + +5. **Edge Action Manager** (`edge_action_manager2.py`) + - Manages execution of edge-specific actions + - Pluggable action system for custom behaviors + - Handles Nav2 action clients (NavigateToPose, NavigateThroughPoses, etc.) + - Implements row operations for agricultural navigation + - Manages boundary detection and edge side edges + - **Complex module**: 1,363 lines, extensive ROS 2 action handling + +### 4. Key Classes and Their Responsibilities + +#### EdgeActionManager2 (`edge_action_manager2.py`) +- **Purpose**: Execute navigation actions for edges, handle agricultural row operations +- **Key Methods**: + - `execute_action()` - Main action execution entry point + - `navigate_to_pose()` - Send Nav2 NavigateToPose actions + - `navigate_through_poses()` - Send Nav2 NavigateThroughPoses actions + - `handle_row_operations()` - Agricultural row navigation logic + - `get_boundary_nodes()` - Determine entry/exit boundary nodes for rows +- **Important**: Integrates with Nav2 action server, handles goal status callbacks + +#### TopologicalMap (`topological_map.py`) +- **Purpose**: In-memory representation of topological map +- **Key Methods**: + - `load_from_yaml()` - Load map from YAML file + - `get_node()` - Retrieve node by name + - `get_edges_from_node()` - Get all edges from a node + - `get_node_neighbours()` - Get neighbouring nodes + +#### TopologicalRouteSearch2 (`route_search2.py`) +- **Purpose**: A* path planning on topological graph +- **Key Methods**: + - `search_route()` - Find optimal path between nodes + - `get_path_cost()` - Calculate path cost with property-based weighting + - `is_node_blocked()` - Check if node is restricted/blocked + +## Development Guidelines + +### Code Style + +**Python**: +- Follow PEP 8 style guide +- Use PEP 257 docstring conventions +- Type hints encouraged for clarity (especially in new code) +- Validated with `ament_flake8` and `ament_pep257` +- Line length: 120 characters (flexible, follow existing file style) + +**C++**: +- Follow ROS 2 C++ style guidelines +- Use modern C++ features (C++14 minimum) +- Follow ROS 2 naming conventions (snake_case for functions/variables) + +**ROS 2 Naming Conventions**: +- Node names: lowercase with underscores (e.g., `topological_navigation`) +- Topic names: lowercase with slashes (e.g., `/topological_map_2`) +- Service/action names: CamelCase (e.g., `NavigateToPose`) + +### Testing + +**Test Structure**: +- `test/` - Unit tests (pytest) +- `tests/` - Integration tests (launch_pytest) +- Test files: `test_*.py` or `*_tester.py` + +**Running Tests**: +```bash +# Run all tests for a package +colcon test --packages-select topological_navigation + +# Run with verbose output +colcon test --packages-select topological_navigation --event-handlers console_direct+ + +# Run specific test +colcon test --packages-select topological_navigation --pytest-args -k test_route_search +``` + +**Writing Tests**: +- Follow existing test patterns in `test/` and `tests/` directories +- Use pytest fixtures for common setup +- Integration tests should use launch_pytest for ROS 2 node lifecycle +- Mock external dependencies (Nav2 action servers, etc.) + +### Building + +```bash +# Build all packages in workspace +cd /path/to/workspace +colcon build + +# Build specific package +colcon build --packages-select topological_navigation + +# Build with symlink install (faster for Python changes) +colcon build --symlink-install + +# Clean build +colcon build --cmake-clean-cache +``` + +**Important**: After building, source the workspace: +```bash +source install/setup.bash +``` + +### Linting + +```bash +# Python linting (flake8) +colcon test --packages-select topological_navigation \ + --event-handlers console_direct+ \ + --pytest-args -k test_flake8 + +# PEP 257 docstring checks +colcon test --packages-select topological_navigation \ + --event-handlers console_direct+ \ + --pytest-args -k test_pep257 + +# Run all linters +colcon test --packages-select topological_navigation \ + --event-handlers console_direct+ +``` + +## Common Tasks for AI Agents + +### Task 1: Add New Property to Topological Maps + +**Goal**: Add a new property to node/edge definitions + +**Steps**: +1. **Update Map Files** (`.tmap2.yaml`): + ```yaml + properties: + my_new_property: value + ``` + +2. **Update Code to Use Property**: + ```python + props = node["node"].get("properties", {}) + my_value = props.get("my_new_property", default_value) + ``` + +3. **Update Documentation**: Add property description to `doc/PROPERTIES.md` + +4. **Consider Schema Update**: If property should be validated, update `config/schema2.json` + +**Key Files**: +- Map YAML files (*.tmap2.yaml) +- `doc/PROPERTIES.md` +- `config/schema2.json` (optional) +- Code files using the property + +### Task 2: Modify Edge Action Behavior + +**Goal**: Change how edges are executed during navigation + +**Key File**: `topological_navigation/edge_action_manager2.py` + +**Common Modifications**: +- Add new action type support +- Modify Nav2 goal parameters +- Add pre/post-action behaviors +- Implement custom action handlers + +**Example Pattern**: +```python +def execute_custom_action(self, edge_data): + """Execute custom edge action""" + # Extract properties + props = edge_data.get("properties", {}) + + # Prepare action goal + goal = CustomAction.Goal() + goal.parameter = props.get("custom_param", default) + + # Send action + self._send_goal(goal) +``` + +**Testing**: Add integration test in `tests/` directory + +### Task 3: Add New ROS 2 Node/Script + +**Goal**: Create a new ROS 2 executable script + +**Steps**: +1. **Create Script**: Add to `topological_navigation/topological_navigation/scripts/` +2. **Add to setup.py**: + ```python + entry_points={ + 'console_scripts': [ + 'my_new_node = topological_navigation.scripts.my_new_node:main', + ], + }, + ``` +3. **Follow ROS 2 Node Pattern**: + ```python + import rclpy + from rclpy.node import Node + + class MyNewNode(Node): + def __init__(self): + super().__init__('my_new_node') + self.get_logger().info('Node started') + + def main(args=None): + rclpy.init(args=args) + node = MyNewNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + ``` + +4. **Add Launch File** (optional): `launch/my_new_node.launch.py` +5. **Add Test**: `test/test_my_new_node.py` + +### Task 4: Fix Navigation Issues + +**Common Issues**: + +1. **Robot not localising correctly**: + - Check `localisation2.py` - verify influence zone configuration + - Check `/closest_node` topic - is closest node being published? + - Verify robot pose is being published to correct topic + +2. **Edge action not executing**: + - Check `edge_action_manager2.py` - verify action client is connected + - Check Nav2 action server is running + - Verify edge action type matches available action servers + - Check edge properties for correct action parameters + +3. **Route planning fails**: + - Check `route_search2.py` - verify graph connectivity + - Check for blocked/restricted nodes + - Verify source and target nodes exist in map + +**Debugging Tools**: +- `/topological_map_2` topic - view current map +- `/current_node` topic - view current localisation +- `/topological_navigation/feedback` - view navigation progress +- RViz markers - visualise topological map + +### Task 5: Update Map Schema + +**Goal**: Modify the structure of topological maps + +**Key File**: `config/schema2.json` + +**Steps**: +1. Update JSON schema with new fields +2. Update map loader (`load_maps_from_yaml.py`) +3. Update map validation logic +4. Update example maps in `tests/` directory +5. Update documentation (`doc/PROPERTIES.md`) +6. Run validation tests to ensure backward compatibility + +## Important Patterns + +### Pattern 1: Safe Property Access + +Always use defensive property access to avoid KeyErrors: + +```python +# Node properties +node_data = topological_map.get_node("node_name") +props = node_data["node"].get("properties", {}) +tolerance = props.get("xy_goal_tolerance", 0.5) # Default 0.5 + +# Nested properties +roboflow = props.get("roboflow", {}) +enabled = roboflow.get("enabled", False) + +# Check existence before use +if "custom_behavior" in props: + behavior = props["custom_behavior"] +else: + behavior = default_behavior +``` + +### Pattern 2: ROS 2 Action Client Usage + +Follow the pattern in `edge_action_manager2.py`: + +```python +from rclpy.action import ActionClient +from nav2_msgs.action import NavigateToPose + +class MyActionHandler: + def __init__(self, node): + self._nav_client = ActionClient( + node, + NavigateToPose, + '/navigate_to_pose' + ) + + def send_goal(self, pose): + # Wait for action server + self._nav_client.wait_for_server() + + # Prepare goal + goal_msg = NavigateToPose.Goal() + goal_msg.pose = pose + + # Send goal with feedback/result callbacks + send_goal_future = self._nav_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback + ) + send_goal_future.add_done_callback(self.goal_response_callback) +``` + +### Pattern 3: Map Loading and Validation + +```python +from topological_navigation.topological_map import TopologicalMap +from topological_navigation.load_maps_from_yaml import load_map_from_yaml + +# Load map +map_data = load_map_from_yaml("path/to/map.tmap2.yaml") + +# Create TopologicalMap object +topo_map = TopologicalMap() +topo_map.load_from_dict(map_data) + +# Access nodes +node = topo_map.get_node("node_name") +edges = topo_map.get_edges_from_node("node_name") +``` + +### Pattern 4: ROS 2 Parameter Handling + +```python +from rcl_interfaces.msg import ParameterDescriptor + +class MyNode(Node): + def __init__(self): + super().__init__('my_node') + + # Declare parameters with defaults + self.declare_parameter( + 'map_file', + '', + ParameterDescriptor(description='Path to topological map YAML') + ) + + # Get parameter value + map_file = self.get_parameter('map_file').value +``` + +## Common Pitfalls + +### Pitfall 1: Property Access Without Defaults +**Wrong**: +```python +tolerance = node["node"]["properties"]["xy_goal_tolerance"] # KeyError if missing! +``` + +**Right**: +```python +props = node["node"].get("properties", {}) +tolerance = props.get("xy_goal_tolerance", 0.5) +``` + +### Pitfall 2: Not Checking Action Server Availability +**Wrong**: +```python +self._nav_client.send_goal_async(goal) # May fail silently +``` + +**Right**: +```python +if not self._nav_client.wait_for_server(timeout_sec=5.0): + self.get_logger().error("Nav2 action server not available") + return False +self._nav_client.send_goal_async(goal) +``` + +### Pitfall 3: Hardcoding Frame IDs +**Wrong**: +```python +pose.header.frame_id = "map" # May not match robot's frame +``` + +**Right**: +```python +pose.header.frame_id = node_data["node"]["parent_frame"] +``` + +### Pitfall 4: Ignoring ROS 2 Callback Groups +For concurrent operations in ROS 2, use appropriate callback groups: + +```python +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup + +# For actions that shouldn't block each other +self.callback_group = ReentrantCallbackGroup() +self._nav_client = ActionClient( + self, + NavigateToPose, + '/navigate_to_pose', + callback_group=self.callback_group +) +``` + +## Key Files Reference + +### Core Navigation Files +- `scripts/navigation2.py` - Main navigation action server (topological navigation entry point) +- `edge_action_manager2.py` - Edge action execution (complex, 1363 lines) +- `route_search2.py` - A* path planning algorithm +- `scripts/localisation2.py` - Topological localisation node +- `scripts/map_manager.py` - Map loading and publishing node + +### Map Data Structures +- `topological_map.py` - In-memory map representation +- `load_maps_from_yaml.py` - YAML map loader +- `manager2.py` - Map management core logic + +### Utility Classes +- `dict_tools` (in edge_action_manager2.py) - Nested dictionary operations +- `tmap_utils.py` - Map manipulation utilities +- `route_search.py` - Legacy route search (v1) + +### Configuration and Validation +- `config/schema2.json` - JSON schema for map validation +- `doc/PROPERTIES.md` - Properties system documentation + +### Testing +- `test/test_navigationcore.py` - Navigation core tests +- `tests/topological_navigation_tester_critical.py` - Critical integration tests +- `tests/map_manager_tester.py` - Map manager tests + +## Agricultural Navigation Specifics + +### Row Operations +The system includes specialized support for agricultural row navigation: + +**Key Features**: +- Boundary node detection for row entry/exit +- Side edge support for parallel row paths +- Roboflow integration for vision-based row detection +- Configurable row operation parameters + +**Usage Pattern**: +```yaml +edges: + - edge_id: entry_to_exit + node: row_exit + action: RowOperation + action_type: nav2_msgs/action/NavigateToPose + properties: + row_type: "vineyard" + roboflow: + enabled: true + confidence: 0.7 + max_speed: 0.5 +``` + +**Implementation**: See `edge_action_manager2.py` → `RowOperations` class integration + +### Boundary Detection +- `get_boundary_nodes()` in EdgeActionManager2 +- Uses side_edges property to identify parallel paths +- Determines optimal entry/exit points for row operations + +## Dependencies and External Systems + +### ROS 2 Navigation Stack (Nav2) +- **Actions Used**: + - `NavigateToPose` - Navigate to single goal + - `NavigateThroughPoses` - Navigate through waypoints + - `FollowWaypoints` - Follow ordered waypoint list + - `ComputePathToPose` - Path planning only + - `ComputePathThroughPoses` - Multi-goal path planning + +- **Integration Point**: `edge_action_manager2.py` +- **Assumption**: Nav2 action servers are running and accessible + +### Coordinate Frame Requirements +- **Base Frame**: Robot's base link (typically `base_link`) +- **Map Frame**: Global reference frame (typically `map`) +- **TF Tree**: Must have valid transforms between frames +- **Node Poses**: Defined in parent_frame (usually `map`) + +### Message Dependencies +- `geometry_msgs` - Pose, PoseStamped, Point, Quaternion +- `nav_msgs` - Odometry, Path +- `std_msgs` - Header, String +- `sensor_msgs` - Various sensor data +- `visualization_msgs` - Marker, MarkerArray (for RViz) + +## Debugging and Troubleshooting + +### Common ROS 2 Commands + +```bash +# List all nodes +ros2 node list + +# View node info +ros2 node info /topological_navigation + +# List topics +ros2 topic list + +# View topic data +ros2 topic echo /topological_map_2 +ros2 topic echo /current_node + +# Check action servers +ros2 action list + +# Send test action goal +ros2 action send_goal /topological_navigation \ + topological_navigation_msgs/action/GotoNode \ + "{target: 'node_name'}" + +# View logs +ros2 run rqt_console rqt_console +``` + +### Log Analysis +- **Info logs**: Normal operation, navigation progress +- **Warn logs**: Recoverable issues, fallback behaviors +- **Error logs**: Action failures, missing data, connectivity issues + +### RViz Visualization +Launch RViz to visualize topological map: +```bash +ros2 launch topological_rviz_tools topological_rviz.launch.py +``` + +**Markers**: +- Red spheres: Nodes +- Green arrows: Edges +- Blue lines: Planned route +- Yellow highlight: Current node + +## CI/CD and Quality Assurance + +### GitHub Actions Workflows +- YAML schema validation +- Python linting (flake8, pep257) +- Unit and integration tests +- Build verification + +### Pre-commit Checks +Before committing, run: +```bash +# Lint +colcon test --packages-select topological_navigation \ + --pytest-args -k "flake8 or pep257" + +# Unit tests +colcon test --packages-select topological_navigation +``` + +## Additional Resources + +### Documentation +- `README.md` - Package overview +- `doc/PROPERTIES.md` - Properties system detailed guide +- `.github/copilot-instructions.md` - GitHub Copilot specific guidance +- `REVIEW.md` - Code review guidelines + +### Example Maps +- `tests/` directory contains example topological maps +- Look for `.tmap2.yaml` files + +### External Links +- ROS 2 Documentation: https://docs.ros.org/ +- Nav2 Documentation: https://navigation.ros.org/ +- ROS 2 Action Documentation: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html + +## Getting Help + +### Internal Resources +1. Check existing code patterns in similar files +2. Read test files for usage examples +3. Review commit history for recent changes: `git log --oneline` + +### External Resources +1. ROS 2 Discourse: https://discourse.ros.org/ +2. Nav2 GitHub Issues: https://github.com/ros-planning/navigation2/issues +3. ROS Answers: https://answers.ros.org/ + +## Agent-Specific Guidelines + +### When Analyzing This Codebase +1. **Start with map structure**: Understand topological maps first +2. **Follow data flow**: Map → Route Search → Navigation → Edge Actions +3. **Check properties**: Always verify what properties are available/required +4. **Test incrementally**: Small changes, test frequently +5. **Respect ROS 2 patterns**: Use established action/topic/service patterns + +### When Making Changes +1. **Preserve backward compatibility**: Especially for map formats +2. **Add tests**: Every new feature needs tests +3. **Update documentation**: Keep AGENTS.md, PROPERTIES.md, README.md in sync +4. **Follow existing patterns**: Don't introduce new patterns without discussion +5. **Consider agricultural use cases**: Changes impact real-world robot operations + +### When Debugging +1. **Start with logs**: Check ROS 2 logs for errors/warnings +2. **Verify map structure**: Ensure YAML maps are valid +3. **Check action servers**: Ensure Nav2 is running +4. **Use RViz**: Visualize to understand spatial relationships +5. **Isolate components**: Test map loading, routing, actions separately + +--- + +**Last Updated**: 2026-01-27 +**Branch**: agent_prep +**Maintainer**: AI Coding Agents diff --git a/README.md b/README.md index 8afd62fe..8333bb1b 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,14 @@ # topological_navigation -A topological navigation planning framework +A topological navigation planning framework for ROS 2. + +## Packages + +This repository contains the following ROS 2 packages: + +- **[topological_navigation](topological_navigation/README.md)** - Core topological navigation and mapping functionality +- **[topological_navigation_msgs](topological_navigation_msgs/)** - Message, service, and action definitions for topological navigation +- **[topological_utils](topological_utils/)** - Utility tools for creating and managing topological maps +- **[topological_rviz_tools](topological_rviz_tools/README.md)** - RViz-based tools for interactive topological map construction and editing + +Please refer to the individual package README files for detailed documentation and usage instructions. diff --git a/REVIEW.md b/REVIEW.md new file mode 100644 index 00000000..037c3732 --- /dev/null +++ b/REVIEW.md @@ -0,0 +1,504 @@ +# Repository Structure Review and Improvement Recommendations + +**Date**: January 2026 +**Repository**: LCAS/topological_navigation +**Version**: 3.0.5 (ROS 2 Humble/Iron) + +This document provides an analysis of the current repository structure, identifies potential issues, and recommends improvements to guide future development and AI-assisted contributions. + +--- + +## Executive Summary + +The topological_navigation repository is a mature ROS 2 project with a well-established foundation. However, analysis reveals several areas for improvement: + +1. **Code Organization**: Presence of legacy "version 2" files alongside original versions creates confusion +2. **Documentation**: Inconsistent documentation coverage across packages +3. **Testing**: Limited test coverage, especially for newer ROS 2 features +4. **Code Duplication**: Some functionality exists in multiple versions (v1 vs v2) +5. **Technical Debt**: Several TODOs and FIXMEs in critical navigation code + +**Overall Assessment**: The repository is functional and well-maintained, but would benefit from modernization and consolidation efforts. + +--- + +## Structural Analysis + +### 1. Code Organization and Versioning + +**Issue: Multiple File Versions** + +The repository contains **15+ files with "2" suffix** (e.g., `localisation2.py`, `manager2.py`, `route_search2.py`), indicating parallel implementations: + +``` +topological_navigation/ +├── manager.py # Legacy version +├── manager2.py # Current version (1538 lines) +├── route_search.py # Legacy version +├── route_search2.py # Current version +├── localisation.py # Legacy version +└── scripts/ + └── localisation2.py # Current version +``` + +**Impact**: +- **Confusion for Contributors**: Unclear which files are authoritative +- **Maintenance Burden**: Bug fixes may need to be applied to multiple versions +- **Code Bloat**: Doubles the codebase size unnecessarily +- **AI Agent Confusion**: AI tools may reference or modify deprecated code + +**Recommendations**: + +1. **Short-term (High Priority)**: + - Add clear deprecation notices in legacy files: + ```python + """ + DEPRECATED: This file is maintained for backward compatibility only. + New development should use manager2.py instead. + Will be removed in version 4.0.0. + """ + ``` + - Update documentation to clarify which versions to use + +2. **Medium-term**: + - Create migration guide for users still on legacy versions + - Rename "2" files to remove suffix once legacy is removed: + - `manager2.py` → `manager.py` + - `route_search2.py` → `route_search.py` + - Consider semantic versioning for major API changes instead of file suffixes + +3. **Long-term (Version 4.0.0)**: + - Remove all legacy files after deprecation period + - Consolidate to single implementation per module + +### 2. Package Structure + +**Current Structure**: Well-organized multi-package workspace + +``` +topological_navigation/ +├── topological_navigation/ # Core (Python, ~8k LOC) +├── topological_navigation_msgs/ # Messages +├── topological_rviz_tools/ # RViz tools (C++/Qt) +└── topological_utils/ # Utilities (Python, ~70+ scripts) +``` + +**Strengths**: +- Clear separation of concerns +- Standard ROS 2 package layout +- Logical grouping of functionality + +**Issues**: + +1. **Script Proliferation**: `topological_utils` contains **70+ Python scripts**, making it difficult to discover and maintain tools + +2. **Inconsistent Naming**: Some scripts use underscores, others don't; some have `.py` extension in the name, others don't + +**Recommendations**: + +1. **Organize Utilities by Function**: + ``` + topological_utils/ + └── topological_utils/ + ├── commands/ # CLI tools + │ ├── map_management/ + │ │ ├── add_node.py + │ │ ├── remove_node.py + │ │ └── crop_map.py + │ ├── visualization/ + │ │ ├── plot_map.py + │ │ └── draw_predictions.py + │ └── migration/ + │ ├── migrate.py + │ └── map_converter.py + └── lib/ # Shared library code + ``` + +2. **Create Unified CLI Tool**: + - Consider a single entry point: `topo` command with subcommands + - Example: `topo map add-node` instead of `add_node.py` + - Reduces cognitive load and improves discoverability + - Similar to `git`, `docker`, or `ros2` command structure + +3. **Consolidate Duplicate Functionality**: + - Multiple map plotting tools exist (`plot_yaml.py`, `plot_yaml2.py`, `plot_topo_map.py`, `plot_topo_map2.py`) + - Consider single `plot` command with options for different formats + +### 3. Documentation + +**Current State**: + +| Package | README Length | Quality | +|---------|---------------|---------| +| Root | 14 lines | Minimal - just package list | +| topological_navigation | 278 lines | Good - comprehensive | +| topological_rviz_tools | 79 lines | Good - usage examples | +| topological_utils | None | **Missing** | +| topological_navigation_msgs | None | **Missing** | + +**Issues**: + +1. **Missing Package READMEs**: Two packages lack dedicated documentation +2. **Outdated Content**: Main README references ROS 1 concepts (MongoDB, `roslaunch`) +3. **No Architecture Diagrams**: Complex system lacks visual documentation +4. **API Documentation**: No generated API docs (Sphinx, Doxygen) + +**Recommendations**: + +1. **Immediate (High Priority)**: + - Add READMEs to `topological_utils` and `topological_navigation_msgs` + - Update root README with: + - Quick start guide for ROS 2 + - System requirements + - Installation instructions + - Link to key documentation + +2. **Short-term**: + - Create `docs/` directory with: + - Architecture overview with diagrams (use Mermaid or PlantUML) + - Component interaction diagrams + - Data flow diagrams + - API reference (auto-generated from code) + +3. **Medium-term**: + - Set up automated documentation generation: + - Python: Sphinx with autodoc + - C++: Doxygen + - Add documentation to CI/CD pipeline + - Host documentation (GitHub Pages, Read the Docs) + +4. **Enhanced Documentation Examples**: + ```markdown + docs/ + ├── index.md # Main documentation hub + ├── getting-started/ + │ ├── installation.md + │ ├── quick-start.md + │ └── tutorials/ + ├── architecture/ + │ ├── overview.md + │ ├── components.md + │ └── diagrams/ + ├── user-guide/ + │ ├── creating-maps.md + │ ├── navigation.md + │ └── properties-guide.md + ├── developer-guide/ + │ ├── contributing.md + │ ├── coding-standards.md + │ └── testing.md + └── api/ + ├── python/ + └── cpp/ + ``` + +### 4. Testing Infrastructure + +**Current State**: + +- **Python**: Uses pytest and launch_pytest ✓ +- **Linters**: ament_flake8, ament_pep257 ✓ +- **CI/CD**: GitHub Actions testing on Humble and Iron ✓ +- **Test Coverage**: Only 1-2 test files found, mostly integration tests + +**Issues**: + +1. **Low Unit Test Coverage**: Very few unit tests for core modules +2. **Legacy Test Documentation**: `tests/README.md` references ROS 1 tools (catkin_make, roslaunch, MongoDB, MORSE simulator) +3. **No Coverage Metrics**: No measurement of code coverage +4. **Manual Testing Required**: Some functionality requires manual verification + +**Recommendations**: + +1. **Immediate**: + - Update `tests/README.md` for ROS 2 (replace catkin_make with colcon, roslaunch with ros2 launch) + - Add unit tests for critical modules: + - `route_search2.py` - path planning algorithms + - `tmap_utils.py` - map utilities + - `restrictions_impl.py` - validation logic + +2. **Short-term**: + - Add code coverage reporting: + ```bash + pytest --cov=topological_navigation --cov-report=html + ``` + - Set coverage targets (aim for 70%+ for core modules) + - Add coverage badge to README + +3. **Medium-term**: + - Create comprehensive test suite: + - Unit tests for all public APIs + - Integration tests for navigation scenarios + - Property validation tests + - Add performance/benchmark tests for route planning + - Mock external dependencies (Nav2, TF) + +4. **Example Test Structure**: + ``` + topological_navigation/ + └── test/ + ├── unit/ + │ ├── test_route_search.py + │ ├── test_map_validation.py + │ └── test_properties.py + ├── integration/ + │ ├── test_navigation_flow.py + │ └── test_map_loading.py + └── fixtures/ + ├── sample_maps/ + └── mock_data/ + ``` + +### 5. Code Quality Issues + +**Technical Debt Identified**: + +1. **TODOs and FIXMEs**: 8+ instances in production code + ```python + # localisation2.py + # TODO: remove Temporary arg until tags functionality is MongoDB independent + + # navigation2.py + # FIXME: not implemented + + # edge_action_manager2.py + #TODO change this to actual + ``` + +2. **Wildcard Imports**: Found in 10+ utility scripts + ```python + from module import * # Anti-pattern + ``` + +3. **Large Files**: Some modules exceed 1000 lines + - `edge_action_manager2.py`: 1362 lines + - `manager2.py`: 1538 lines + +**Recommendations**: + +1. **Immediate**: + - Create GitHub issues for each TODO/FIXME + - Add issue references in code: + ```python + # TODO(#123): remove Temporary arg until tags functionality is MongoDB independent + ``` + +2. **Short-term**: + - Refactor wildcard imports to explicit imports + - Add pre-commit hooks to prevent new wildcard imports + - Configure linters to flag these issues + +3. **Medium-term**: + - Refactor large files into smaller, focused modules: + ```python + # Instead of edge_action_manager2.py (1362 lines) + edge_action_manager/ + ├── __init__.py + ├── base.py # Base classes + ├── actions.py # Action implementations + ├── state.py # State management + └── utils.py # Helper functions + ``` + - Apply SOLID principles for better maintainability + +### 6. Properties System (Positive Example) + +**Strengths**: + +- Excellent documentation in `PROPERTIES.md` ✓ +- Flexible, extensible design ✓ +- Backward compatible ✓ +- Well-thought-out namespacing approach ✓ + +**Minor Recommendations**: + +1. Add JSON Schema for property validation (optional but helpful) +2. Create property builder/validator classes for common patterns +3. Add examples directory with real-world property configurations + +### 7. Dependency Management + +**Current State**: +- Uses standard ROS 2 dependencies ✓ +- Minimal external dependencies ✓ +- Clear dependency declarations in `package.xml` ✓ + +**Potential Issues**: + +1. **No Dependency Pinning**: Not using specific versions +2. **No Security Scanning**: No automated vulnerability checks + +**Recommendations**: + +1. Add Dependabot or Renovate for dependency updates +2. Add security scanning to CI/CD (e.g., `safety` for Python) +3. Document tested dependency versions + +### 8. Potential Performance Concerns + +**Areas to Monitor**: + +1. **Large Map Handling**: Route planning on graphs with hundreds of nodes +2. **Visualization Performance**: Marker updates with many nodes/edges +3. **Memory Usage**: Loading multiple large maps simultaneously + +**Recommendations**: + +1. Add performance benchmarks for route planning +2. Implement map caching strategies +3. Consider lazy loading for large maps +4. Profile memory usage in long-running scenarios + +--- + +## Priority Recommendations Summary + +### High Priority (Address Soon) + +1. ✅ **Add deprecation notices** to legacy "version 1" files +2. ✅ **Create READMEs** for topological_utils and topological_navigation_msgs +3. ✅ **Update test documentation** for ROS 2 +4. ✅ **Convert TODOs to GitHub issues** with tracking +5. ✅ **Add basic unit tests** for core modules + +### Medium Priority (Next 6 Months) + +1. 📋 **Plan legacy code removal** for version 4.0.0 +2. 📋 **Refactor large files** (>1000 lines) into smaller modules +3. 📋 **Organize utility scripts** into logical subdirectories +4. 📋 **Generate API documentation** (Sphinx/Doxygen) +5. 📋 **Add code coverage** reporting and targets + +### Low Priority (Future Enhancements) + +1. 💡 **Create unified CLI tool** (`topo` command) +2. 💡 **Add architecture diagrams** to documentation +3. 💡 **Implement performance benchmarks** +4. 💡 **Add dependency security scanning** +5. 💡 **Create migration guides** between versions + +--- + +## Code Modernization Opportunities + +### Python Code + +1. **Type Hints**: Add comprehensive type hints to all public APIs + ```python + def find_route(start: str, end: str, map_name: str) -> List[str]: + """Find route between nodes.""" + ... + ``` + +2. **Dataclasses**: Use dataclasses for data structures + ```python + from dataclasses import dataclass + + @dataclass + class Node: + name: str + pose: Pose + properties: Dict[str, Any] + edges: List[Edge] + ``` + +3. **Async/Await**: Consider async patterns for I/O-bound operations + +### C++ Code + +1. **Modern C++**: Ensure consistent use of C++14/17 features +2. **Smart Pointers**: Verify consistent use of shared_ptr/unique_ptr +3. **RAII**: Ensure proper resource management + +--- + +## AI Agent Friendliness Assessment + +**Current Score: 7/10** + +**Strengths**: +- ✅ Clear package structure +- ✅ Excellent properties documentation +- ✅ Standard ROS 2 patterns +- ✅ Consistent Python style (PEP 8) + +**Areas for Improvement**: +- ⚠️ Legacy code confusion (version 1 vs 2) +- ⚠️ Limited inline documentation +- ⚠️ Missing API documentation +- ⚠️ Few code examples for common tasks + +**To Improve AI Agent Experience**: + +1. **Add more docstrings** with examples: + ```python + def add_edge(node_from: str, node_to: str, action: str) -> None: + """Add edge between two nodes. + + Args: + node_from: Source node name + node_to: Target node name + action: Action to execute (e.g., "NavigateToPose") + + Example: + >>> add_edge("Start", "Goal", "NavigateToPose") + + Raises: + ValueError: If either node doesn't exist + """ + ``` + +2. **Create examples directory**: + ``` + examples/ + ├── basic_navigation.py + ├── custom_properties.py + ├── map_creation.py + └── edge_actions.py + ``` + +3. **Add inline comments for complex algorithms** (e.g., A* implementation) + +4. **Provide migration examples** between versions + +--- + +## Conclusion + +The topological_navigation repository is a well-maintained, functional ROS 2 project with a solid foundation. The primary recommendations focus on: + +1. **Reducing confusion** by deprecating and eventually removing legacy code +2. **Improving discoverability** through better documentation and organization +3. **Increasing confidence** through comprehensive testing +4. **Modernizing code** to leverage current best practices + +These improvements will benefit both human developers and AI coding agents, making the repository easier to understand, maintain, and extend. + +**Estimated Effort**: +- High Priority Items: ~2-3 weeks of focused work +- Medium Priority Items: ~1-2 months spread over multiple sprints +- Low Priority Items: Ongoing improvements over 6-12 months + +**Expected Benefits**: +- Reduced onboarding time for new contributors +- Fewer bugs from using deprecated code +- Better AI agent assistance +- Improved maintainability and extensibility +- Higher confidence in code quality + +--- + +## Appendix: Metrics Summary + +| Metric | Value | Target | +|--------|-------|--------| +| Total Python Files | 144 | - | +| Files with "2" suffix | 15 | 0 (eventually) | +| Utility Scripts | ~70 | Consolidated | +| Test Files | ~2 | 20+ | +| Code Coverage | Unknown | 70%+ | +| TODOs/FIXMEs | 8+ | 0 (converted to issues) | +| Packages with READMEs | 2/4 | 4/4 | +| API Documentation | No | Yes | +| CI/CD Pipelines | 2 | 3+ (add coverage) | + diff --git a/topological_navigation/README.md b/topological_navigation/README.md index b9e97d0f..240f9eed 100644 --- a/topological_navigation/README.md +++ b/topological_navigation/README.md @@ -3,7 +3,11 @@ Topological Navigation **These instructions are intended for the legacy branch of topological navigation. Instructions for the master branch (topological navigation 2) are to do.** -This node provides support for topological navigation in the STRANDS system. +This package provides support for topological navigation, originally devised for the STRANDS system. + +## Flexible Properties System + +The topological map schema now supports a flexible properties system that allows you to attach custom metadata to nodes and edges without requiring schema modifications. This enables domain-specific customization such as setting navigation tolerances, semantic labels, operational zones, and application-specific attributes. See [doc/PROPERTIES.md](doc/PROPERTIES.md) for detailed documentation. This module requires: * move_base diff --git a/topological_navigation/config/template_edge.yaml b/topological_navigation/config/template_edge.yaml index 12bd5a47..04fb1b76 100644 --- a/topological_navigation/config/template_edge.yaml +++ b/topological_navigation/config/template_edge.yaml @@ -10,6 +10,7 @@ goal: frame_id: $node.parent_frame pose: $node.pose node: destination +properties: {} recovery_behaviours_config: '' restrictions_planning: 'True' restrictions_runtime: 'True' \ No newline at end of file diff --git a/topological_navigation/config/tmap-schema.yaml b/topological_navigation/config/tmap-schema.yaml index f372675d..412a262e 100644 --- a/topological_navigation/config/tmap-schema.yaml +++ b/topological_navigation/config/tmap-schema.yaml @@ -64,9 +64,17 @@ properties: type: string node: type: string + properties: + type: object + additionalProperties: true + description: Flexible dictionary of application-specific edge properties (e.g., max_speed, priority, surface_type) name: type: string parent_frame: type: string + properties: + type: object + additionalProperties: true + description: Flexible dictionary of application-specific node properties (e.g., xy_goal_tolerance, yaw_goal_tolerance, semantics, zone) pointset: type: string diff --git a/topological_navigation/doc/PROPERTIES.md b/topological_navigation/doc/PROPERTIES.md new file mode 100644 index 00000000..fc7de101 --- /dev/null +++ b/topological_navigation/doc/PROPERTIES.md @@ -0,0 +1,375 @@ +# Flexible Node and Edge Properties System + +This document describes the flexible properties system for topological maps, which allows application-specific metadata to be attached to both nodes and edges without requiring schema modifications. + +## Overview + +The topological map schema supports optional `properties` dictionaries for both nodes and edges. These properties enable domain-specific customisation while maintaining backwards compatibility with existing maps. + +## Node Properties + +Node properties are defined in `nodes[].node.properties` as a YAML dictionary. The schema allows any key-value pairs. + +### Default Properties + +The following properties are commonly used for navigation control: + +| Property | Type | Description | +|----------|------|-------------| +| `xy_goal_tolerance` | float | XY position tolerance for goal reaching (metres) | +| `yaw_goal_tolerance` | float | Yaw orientation tolerance for goal reaching (radians) | + +### Example Custom Properties + +| Property | Type | Description | +|----------|------|-------------| +| `row` | integer | Row identifier (e.g., for agricultural polytunnel scenarios) | +| `semantics` | string | Semantic meaning of the node (e.g., "charging_station", "inspection_point") | +| `zone` | string | Operational zone designation | +| `access_level` | string | Permission level required for access | +| `capacity` | integer | Maximum number of robots that can occupy the node | + +### Node Properties Example + +```yaml +nodes: +- meta: + map: riseholme + node: ChargingStation1 + pointset: riseholme + node: + name: ChargingStation1 + parent_frame: map + pose: + position: {x: 10.0, y: 5.0, z: 0.0} + orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0} + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: "charging_station" + row: 3 + zone: "A" + capacity: 2 + edges: [] +``` + +## Edge Properties + +Edge properties are defined in `nodes[].node.edges[].properties` as a YAML dictionary. The schema allows any key-value pairs. + +### Example Edge Properties + +| Property | Type | Description | +|----------|------|-------------| +| `max_speed` | float | Maximum traversal speed (m/s) | +| `priority` | integer | Preference weight for path planning (higher = more preferred) | +| `width` | float | Physical width of the traversable path (metres) | +| `surface_type` | string | Terrain classification (e.g., "concrete", "grass", "gravel") | +| `bidirectional` | boolean | Whether the edge can be traversed in both directions | +| `weather_restrictions` | list | Conditions under which edge should not be used | + +### Edge Properties Example + +```yaml +edges: +- edge_id: ChargingStation1_WayPoint2 + node: WayPoint2 + action: NavigateToPose + action_type: geometry_msgs/PoseStamped + properties: + max_speed: 0.5 + priority: 10 + surface_type: "concrete" + bidirectional: true + weather_restrictions: ["heavy_rain", "snow"] + # ... other edge fields +``` + +## Backwards Compatibility + +The `properties` field is optional for both nodes and edges. Existing topological maps without properties remain valid and will continue to work without modification. + +When properties are not specified: +- Node properties default to standard navigation tolerances if needed by the navigation system +- Edge properties are simply absent (empty dictionary) + +## Usage Guidelines + +### Naming Conventions + +- Use `snake_case` for property names +- Use descriptive, domain-appropriate names +- Avoid abbreviations unless they are widely understood + +### Using Namespaces for Properties + +To organise properties by functional area or application, use **namespaces**. A namespace is simply a nested dictionary that groups related properties together. This approach: + +- Prevents naming conflicts between different systems +- Makes it clear which application or module owns each property +- Improves readability and maintainability +- Supports both domain-based and package-based organisation + +#### Namespace Structure + +```yaml +properties: + namespace_name: + property1: value1 + property2: value2 +``` + +#### Namespace Organisation Approaches + +You can organise namespaces in different ways depending on your needs: + +1. **Domain-based namespaces** (by concept or functional area): + ```yaml + properties: + restrictions: # Physical and access constraints + capacity: 2 + max_external_width: 0.8 + semantics: # Semantic labels and classifications + zone: "warehouse" + type: "charging_station" + ``` + +2. **Package-based namespaces** (by ROS package): + ```yaml + properties: + my_fleet_manager: + priority_zone: true + my_safety_module: + emergency_stop_point: false + ``` + +3. **Flat structure** (no namespaces): + ```yaml + properties: + capacity: 2 + zone: "warehouse" + ``` + +All three approaches are valid. Choose the organisation that best fits your application's needs and team conventions. + +#### Common Namespace Examples + +**Domain-based namespaces:** + +| Namespace | Purpose | Example Properties | +|-----------|---------|-------------------| +| `restrictions` | Physical and access constraints | `capacity`, `max_external_width`, `max_external_height`, `access_level` | +| `semantics` | Semantic labels and classifications | `zone`, `type`, `features`, `environmental_conditions` | +| `navigation` | Navigation-specific parameters | `xy_goal_tolerance`, `yaw_goal_tolerance` | +| `safety` | Safety-related constraints | `restricted_hours`, `emergency_access`, `hazard_zones` | + +**Application/package-based namespaces:** + +| Namespace | Purpose | Example Properties | +|-----------|---------|-------------------| +| `fleet_management` | Multi-robot coordination properties | `priority_zone`, `queue_enabled` | +| `logistics` | Warehouse/delivery application properties | `pickup_point`, `dropoff_point`, `storage_type` | +| `agriculture` | Agricultural robotics properties | `row_id`, `crop_type`, `irrigation_zone` | + +#### Namespaced Node Properties Example + +**Package-based organisation:** +```yaml +node: + name: ChargingStation1 + properties: + navigation: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + fleet_management: + capacity: 2 + priority_zone: true + queue_enabled: true + semantics: + type: "charging_station" + zone: "A" + logistics: + pickup_point: false + dropoff_point: false +``` + +**Domain-based organisation (alternative):** +```yaml +node: + name: PolytunnelRow4_Col2 + properties: + restrictions: + capacity: 1 + max_external_width: 0.8 # metres + max_external_height: 1.5 # metres + min_internal_width: 1.2 # metres + min_internal_height: 2.0 # metres + access_level: "operator" + semantics: + zone: + labels: ['polytunnel', 'growing_area'] + details: {tunnel_id: 4, row_id: 5, column_idx: 2} + features: + labels: ['irrigation_point', 'sensor_node'] + details: {sensor_types: ['humidity', 'temperature']} + environmental: + labels: ['soil', 'shade'] + details: {soil_type: 'loam', 'shade_percentage': 30} +``` + +#### Namespaced Edge Properties Example + +**Package-based organisation:** +```yaml +edges: +- edge_id: ChargingStation1_WayPoint2 + node: WayPoint2 + properties: + navigation: + max_speed: 0.5 + safety: + width: 1.2 + restricted_hours: ["22:00-06:00"] + logistics: + priority: 10 + surface_type: "concrete" +``` + +**Domain-based organisation (alternative):** +```yaml +edges: +- edge_id: Row4_Col2_Row4_Col3 + node: Row4_Col3 + properties: + restrictions: + max_external_width: 0.75 # metres - narrow passage + max_external_height: 1.8 # metres - low overhead + capacity: 1 # single robot at a time + semantics: + zone: + labels: ['polytunnel_interior', 'growing_area'] + details: {tunnel_id: 4} + environmental: + labels: ['grass', 'uneven'] + details: {surface_quality: 'variable'} +``` + +#### Accessing Namespaced Properties in Code + +```python +# Safe access to namespaced node properties +node_props = node["node"].get("properties", {}) + +# Access domain-based namespace +restrictions = node_props.get("restrictions", {}) +capacity = restrictions.get("capacity", 1) +max_width = restrictions.get("max_external_width") + +# Access package-based namespace +fleet_props = node_props.get("fleet_management", {}) +priority_zone = fleet_props.get("priority_zone", False) + +# Access nested semantic properties +semantics = node_props.get("semantics", {}) +zone_info = semantics.get("zone", {}) +zone_labels = zone_info.get("labels", []) +zone_details = zone_info.get("details", {}) + +# Access namespaced edge properties +edge_props = edge.get("properties", {}) +restrictions = edge_props.get("restrictions", {}) +max_width = restrictions.get("max_external_width") +``` + +#### Guidelines for Namespace Usage + +1. **Choose the right organisation for your use case**: + - Use **domain-based namespaces** (like `restrictions`, `semantics`) when organising by conceptual categories + - Use **package-based namespaces** when multiple ROS packages share the same map and need to avoid conflicts + - Use **flat structure** for simple cases with few properties + +2. **Be consistent within your project**: Choose one organisational approach and stick with it across your maps. + +3. **Document your namespace conventions**: If you introduce new namespaces, document their purpose and expected properties in your project documentation. + +4. **Avoid deep nesting**: One level of namespace nesting is usually sufficient. Deeper nesting (like `properties.semantics.zone.labels`) should be reserved for truly hierarchical data. + +5. **Backwards compatibility**: For core navigation properties like `xy_goal_tolerance`, either keep them at root level or consistently place them in a `navigation` namespace across all maps. + +6. **Package name convention** (optional): ROS packages that define properties can use their package name as a namespace to avoid conflicts. E.g., a package called "topfleets_coordinator" could use this as the namespace. + +### Type Flexibility + +Properties support various data types: +- **Strings**: `"charging_station"`, `"concrete"` +- **Numbers**: `0.5`, `10`, `3.14` +- **Booleans**: `true`, `false` +- **Lists**: `["heavy_rain", "snow"]` +- **Nested Objects**: `{min: 0.1, max: 1.0}` + +### Application-Specific Properties + +Applications can define their own property schemas and document them appropriately. The topological navigation system will safely ignore properties it does not recognise, allowing different applications to coexist on the same map. + +### Accessing Properties in Code + +When accessing properties programmatically, always check for property existence before use: + +```python +# Safe access to node properties +node_props = node["node"].get("properties", {}) +xy_tolerance = node_props.get("xy_goal_tolerance", 0.3) # Default to 0.3 +semantics = node_props.get("semantics") # Returns None if not present + +# Safe access to edge properties +edge_props = edge.get("properties", {}) +max_speed = edge_props.get("max_speed") # Returns None if not present +if max_speed is not None: + # Use max_speed for navigation control + pass +``` + +## Schema Reference + +The properties fields are defined in `config/tmap-schema.yaml`: + +```yaml +# Node properties (at nodes[].node.properties) +properties: + type: object + additionalProperties: true + description: Flexible dictionary of application-specific node properties + +# Edge properties (at nodes[].node.edges[].properties) +properties: + type: object + additionalProperties: true + description: Flexible dictionary of application-specific edge properties +``` + +## Validating Topological Maps + +The `validate_map.py` script can be used to validate topological map YAML files against the schema: + +```bash +# Basic validation +ros2 run topological_navigation validate_map.py my_map.tmap2.yaml + +# With verbose output +ros2 run topological_navigation validate_map.py my_map.tmap2.yaml -v + +# With custom schema file +ros2 run topological_navigation validate_map.py my_map.tmap2.yaml --schema custom_schema.yaml +``` + +The validator will: +- Check the map structure against the JSON schema +- Report any missing required fields +- Warn about duplicate node names +- Warn about edges pointing to non-existent nodes + +## Related Resources + +- [Topological Map Schema](../config/tmap-schema.yaml) +- [Node Template](../config/template_node_2.yaml) +- [Edge Template](../config/template_edge.yaml) diff --git a/topological_navigation/setup.py b/topological_navigation/setup.py index 1b61be9c..0cb18c5a 100644 --- a/topological_navigation/setup.py +++ b/topological_navigation/setup.py @@ -52,6 +52,7 @@ 'manual_topomapping.py = topological_navigation.scripts.manual_topomapping:main', 'occupancy_checker.py = topological_navigation.scripts.occupancy_checker:main', 'topological_visual.py = topological_navigation.scripts.topological_visual:main', + 'validate_map.py = topological_navigation.scripts.validate_map:main', ], }, diff --git a/topological_navigation/topological_navigation/edge_action_manager2.py b/topological_navigation/topological_navigation/edge_action_manager2.py index 4c5f2b38..93af6631 100644 --- a/topological_navigation/topological_navigation/edge_action_manager2.py +++ b/topological_navigation/topological_navigation/edge_action_manager2.py @@ -155,14 +155,18 @@ def init(self, ACTIONS, route_search, update_params_control_server, inrow_step_s self.update_params_control_server = update_params_control_server self.current_robot_pose = None + self.is_inside_tunnel = False self.odom_sub = self.create_subscription(Odometry, '/odometry/global', self.odom_callback, QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT)) self.get_current_node_sub = self.create_subscription(String, 'closest_node', self.set_current_pose , QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT)) + self.robot_nav_area_sub = self.create_subscription(String, '/robot_navigation_area', self.nav_area_callback, 10) self.boundary_publisher = self.create_publisher(Path, '/boundary_checker', qos_profile=self.latching_qos) self.robot_current_status_pub = self.create_publisher(String, '/robot_operation_current_status', qos_profile=self.latching_qos) self.current_dest = self.create_publisher(String, '/topological_navigation/current_destination', qos_profile=self.latching_qos) + self.target_edge_path_pub = self.create_publisher(Path, "/target_edge_path", qos_profile=self.latching_qos) + self.center_node_pose_pub = self.create_publisher(PoseStamped, "/center_node/pose", qos_profile=self.latching_qos) self.robot_current_behavior_pub = None self.current_node = None @@ -177,7 +181,12 @@ def set_current_pose(self, msg): def odom_callback(self, msg): self.current_robot_pose = msg.pose - + + + def nav_area_callback(self, msg): + self.nav_area = msg.data + self.is_inside_tunnel = (self.nav_area == 'INSIDE_POLYTUNNEL') + def get_nav_action_server_status(self, ): return self.ACTIONS.status_mapping @@ -196,7 +205,32 @@ def get_goal_cancel_error_msg(self, status_code): except Exception as e: self.get_logger().error("Goal cancel code {}".format(status_code)) return self.ACTIONS.goal_cancel_error_codes[0] - + + def _is_waypoint_name(self, name: str) -> bool: + n = (name or "").lower() + return ("waypoint" in n) or n.startswith("wp") or ("_wp" in n) + + def _is_row_node_name(self, name: str) -> bool: + # Uses your existing constant that you already rely on elsewhere + # (you used startswith(ROW_COLUMN_START_INDEX) in execute_row_operation_action) + if not name: + return False + n = str(name) + return n.split("-")[-1].startswith(self.ACTIONS.ROW_COLUMN_START_INDEX) and (not self._is_waypoint_name(n)) + + def _yaw_from_quat(self, q) -> float: + # q: dict with x,y,z,w + x, y, z, w = q["x"], q["y"], q["z"], q["w"] + siny = 2.0 * (w * z + x * y) + cosy = 1.0 - 2.0 * (y * y + z * z) + return math.atan2(siny, cosy) + + def _set_pose_yaw(self, pose_stamped: PoseStamped, yaw: float): + pose_stamped.pose.orientation.x = 0.0 + pose_stamped.pose.orientation.y = 0.0 + pose_stamped.pose.orientation.z = math.sin(yaw / 2.0) + pose_stamped.pose.orientation.w = math.cos(yaw / 2.0) + def _adjust_orientations_for_next_wp(self, poses_dict): """ @@ -231,7 +265,6 @@ def _adjust_orientations_for_next_wp(self, poses_dict): self.get_logger().info("[_adjust_orientations_for_next_wp] Adjusted orientation for pose {} to face next goal".format(i)) return poses_dict - def _process_and_segment_edges(self, edge, destination_node, origin_node, is_execpolicy): """ @@ -346,17 +379,19 @@ def get_goal_align_if(self, edge_id, current_action, next_edge_id=None): if len(next_edge_ids) == 2: next_goal_stage = next_edge_ids[1].split("-") if len(next_goal_stage) == 2: - if (next_goal_stage[1] in self.ACTIONS.GOAL_ALIGN_INDEX) or (next_goal_stage[1] not in self.ACTIONS.GOAL_ALIGN_GOAL): + if (next_goal_stage[1] in self.ACTIONS.GOAL_ALIGN_INDEX) or \ + (next_goal_stage[1] not in self.ACTIONS.GOAL_ALIGN_GOAL): return current_action elif len(next_goal_stage) == 1: - if(current_action == self.ACTIONS.ROW_TRAVERSAL): + if current_action == self.ACTIONS.ROW_TRAVERSAL: return current_action if len(edges) == 2: goal = edges[1] goal_stage = goal.split("-") if len(goal_stage) == 2: - if goal_stage[1] in self.ACTIONS.GOAL_ALIGN_INDEX: - return self.ACTIONS.GOAL_ALIGN + if goal_stage[1] in self.ACTIONS.GOAL_ALIGN_INDEX and not self.is_inside_tunnel: + return self.ACTIONS.GOAL_ALIGN + return current_action @@ -487,10 +522,8 @@ def check_edges_area_same(self, side_edges): def check_target_is_same(self, node1, node2): target1 = np.array([node1["pose"]["position"]["x"], node1["pose"]["position"]["y"]]) - target2 = np.array([node2["pose"]["position"]["x"], node1["pose"]["position"]["y"]]) - if(np.linalg.norm(target1-target2) < 0.001): - return True - return False + target2 = np.array([node2["pose"]["position"]["x"], node2["pose"]["position"]["y"]]) + return np.linalg.norm(target1 - target2) < 0.001 def two_smallest_indices(self, lst): if len(lst) < 1: @@ -506,97 +539,348 @@ def two_smallest_indices(self, lst): def extract_number(self, s): return float(s.split('-')[0][1:]) + def publish_target_edges_as_path(self, selected_edges_dict): + """ + Convert selected edges dictionary to a ROS Path and publish. + Safely handles placeholder values like '$node.pose' by substituting + with actual node pose from selected_edges_dict['node']['pose']. + """ + path_msg = Path() + node_info = selected_edges_dict.get("node", {}) + node_pose = node_info.get("pose", {}) + parent_frame = node_info.get("parent_frame", "map") # fallback to 'map' - def get_navigate_through_poses_goal(self, poses, actions, edge_ids, is_execpolicy=False): - - def handle_row_operation(): - self.target_row_edge_id = edge_id.split("_")[0] - tag_id = self.target_row_edge_id.split("-")[1] - self.target_row_edge_id = self.target_row_edge_id[:-1] + self.ACTIONS.ROW_START_INDEX - tag_id = tag_id[:-1] + self.ACTIONS.ROW_START_INDEX - - self.get_logger().info("Action in_row_operation ") - self.get_logger().info("Edge id and tag id {} {}".format(self.target_row_edge_id, tag_id)) - cen = self.route_search.get_node_from_tmap2(self.target_row_edge_id) - children = self.route_search.get_connected_nodes_tmap2(cen) - selected_row_edge_nodes = {} - for next_edge in children: - if(next_edge.startswith(self.ACTIONS.OUTSIDE_EDGE_START_INDEX)): - upper_nodes = self.route_search.get_node_from_tmap2(next_edge)["node"]["edges"] - for edges_all in upper_nodes: - if((self.ACTIONS.GOAL_ALIGN_INDEX[0] in edges_all["node"]) - and (self.target_row_edge_id not in edges_all["node"])): - - targte_pose = self.route_search.get_node_from_tmap2(edges_all["node"])["node"]["pose"] - targte_pose_x_y = np.array([targte_pose["position"]["x"], targte_pose["position"]["y"]]) - selected_row_edge_nodes[edges_all["node"]] = (targte_pose, targte_pose_x_y) - - center_pose = self.route_search.get_node_from_tmap2(self.target_row_edge_id)["node"]["pose"]["position"] - center_pose = np.array([center_pose["x"], center_pose["y"]]) - distance_vector = [] - distance_with_edge_ids = {} - index_dis = 0 - for the_key, the_value in selected_row_edge_nodes.items(): - distance_vector.append(np.linalg.norm(center_pose- the_value[1])) - distance_with_edge_ids[index_dis] = the_key - index_dis += 1 - min_indices = self.two_smallest_indices(distance_vector) - children = [] - for index in min_indices: - children.append(distance_with_edge_ids[index]) - - # Sort the list based on the extracted number in descending order - children = sorted(children, key=self.extract_number, reverse=True) - - self.get_logger().info("Children edges {}".format(children)) - - target_pose_frame_id = nodes[0]["target_pose"]["header"]["frame_id"] - last_goal = nodes[-1] - - if(len(nodes) > 1): - if(self.check_target_is_same(cen["node"], last_goal["target_pose"])): - last_goal = nodes[0] - - selected_last_node = last_goal - if(len(nodes) == 1): - edges = edge_id.split("_") - if(len(edges) == 2): - edge_0, edge_1 = edges[0], edges[1] - tag_0, tag_1 = edge_0.split("-")[1], edge_1.split("-")[1] - if(tag_1 == tag_id): - selected_last_node["target_pose"]["pose"] = self.route_search.get_node_from_tmap2(edge_0)["node"]["pose"] - elif(tag_0 == tag_id): - selected_last_node["target_pose"]["pose"] = self.route_search.get_node_from_tmap2(edge_1)["node"]["pose"] - elif(len(selected_last_node) == 0 and self.current_node is not None): - selected_last_node["target_pose"]["pose"] = self.route_search.get_node_from_tmap2(self.current_node)["node"]["pose"] - else: - self.get_logger().error("Cound not find bounday edge...") - - self.selected_edges = {} - for child in children: - if tag_id in child: - child_node = self.route_search.get_node_from_tmap2(child) - side_intermediate_pose = self.get_intermediate_pose(cen, child_node, target_pose_frame_id) - side_last_intermediate_pose = self.get_last_intermediate_pose(side_intermediate_pose, cen, selected_last_node) - self.selected_edges[child] = [side_intermediate_pose, side_last_intermediate_pose] - - if(len(self.selected_edges) == 1): - self.selected_edges["side_wall"] = self.get_intermediate_poses_interpolated(self.selected_edges, cen, selected_last_node) - - if(self.check_edges_area_same(self.selected_edges)): - edge_action_is_valid = True - self.get_logger().error("Bounday edges are same") + path_msg.header.frame_id = parent_frame + path_msg.header.stamp = self.get_clock().now().to_msg() + + if not node_pose: + self.get_logger().warn("Node pose missing, cannot publish path") + return + + valid_poses_count = 0 + + for edge in node_info.get("edges", []): + target_pose_data = edge.get("goal", {}).get("target_pose", {}) + + # Check if pose is placeholder + if target_pose_data.get("pose") == "$node.pose": + pose_source = node_pose + elif isinstance(target_pose_data.get("pose"), dict): + pose_source = target_pose_data["pose"] else: - edge_action_is_valid = True - if edge_action_is_valid: - action_msg.setSideEdges(self.selected_edges, target_pose_frame_id) - if self.is_row_boundary_published == False: - boundary_info = action_msg.getBoundary() - self.boundary_publisher.publish(boundary_info) - self.is_row_boundary_published = True - action = self.ACTIONS.ROW_OPERATION - return action, action_msg + self.get_logger().warn( + f"Skipping edge {edge.get('edge_id')} due to invalid target_pose" + ) + continue + + # Create PoseStamped + try: + pose_msg = PoseStamped() + pose_msg.header.frame_id = parent_frame + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.pose.position.x = pose_source["position"]["x"] + pose_msg.pose.position.y = pose_source["position"]["y"] + pose_msg.pose.position.z = pose_source["position"]["z"] + pose_msg.pose.orientation.x = pose_source["orientation"]["x"] + pose_msg.pose.orientation.y = pose_source["orientation"]["y"] + pose_msg.pose.orientation.z = pose_source["orientation"]["z"] + pose_msg.pose.orientation.w = pose_source["orientation"]["w"] + except KeyError as e: + self.get_logger().warn( + f"Skipping edge {edge.get('edge_id')} due to missing key: {e}" + ) + continue + + path_msg.poses.append(pose_msg) + valid_poses_count += 1 + + self.get_logger().info(f"Publishing Path with {valid_poses_count} poses") + self.target_edge_path_pub.publish(path_msg) + + def _publish_empty_boundary(self, frame_id: str): + """Publish an empty boundary path.""" + empty = Path() + empty.header.frame_id = frame_id + empty.header.stamp = self.get_clock().now().to_msg() + self.boundary_publisher.publish(empty) + + def _get_row_center_node(self, edge_id: str): + """ + Parse edge_id to find the row center node. + Returns: (center_node, tag_id, target_row_edge_id) or (None, None, None) on failure. + """ + try: + # edge_id = r7.5-c20_r7.5-c19 + target_row_edge_id_raw = edge_id.split("_")[0] # e.g. 'r7.5-c20' + tag_id = target_row_edge_id_raw.split("-")[1] # e.g. 'c20' + # print trget_row_edge_id_raw, tag_id + self.get_logger().info(f"[_get_row_center_node] Parsed edge_id='{edge_id}' to target_row_edge_id_raw='{target_row_edge_id_raw}', tag_id='{tag_id}'") + # Force ROW_START_INDEX by replacing the last character + # e.g. 'r7.5-c20' -> 'r7.5-c2a' if ROW_START_INDEX='a' + target_row_edge_id = target_row_edge_id_raw.split("c", 1)[0] + "c" + self.ACTIONS.ROW_START_INDEX # target_row_edge_id_raw[:-1] + self.ACTIONS.ROW_START_INDEX + tag_id = tag_id[0] + self.ACTIONS.ROW_START_INDEX + #print target_row_edge_id, tag_id + self.get_logger().info(f"[_get_row_center_node] Adjusted to target_row_edge_id='{target_row_edge_id}', tag_id='{tag_id}'") + except Exception as e: + self.get_logger().error(f"[_get_row_center_node] Failed to parse edge_id='{edge_id}': {e}") + return None, None, None + + cen = self.route_search.get_node_from_tmap2(target_row_edge_id) + if not cen or "node" not in cen or "pose" not in cen["node"]: + self.get_logger().error(f"[_get_row_center_node] Could not resolve '{target_row_edge_id}'") + return None, None, None + + return cen, tag_id, target_row_edge_id + + def _collect_boundary_candidates(self, cen, target_row_edge_id: str): + """ + Collect candidate boundary nodes from connected WayPoint nodes. + Returns: dict of node_id -> (pose_dict, xy_np) + """ + candidates = {} + children = self.route_search.get_connected_nodes_tmap2(cen) or [] + + for next_edge in children: + if not next_edge.startswith(self.ACTIONS.OUTSIDE_EDGE_START_INDEX): + continue + + next_edge_node = self.route_search.get_node_from_tmap2(next_edge) + if not next_edge_node or "node" not in next_edge_node: + continue + + for edge_info in next_edge_node["node"].get("edges", []): + node_id = edge_info.get("node", "") + if not node_id or not isinstance(node_id, str): + continue + # Must contain GOAL_ALIGN_INDEX and not be the target row itself + if self.ACTIONS.GOAL_ALIGN_INDEX[0] not in node_id: + continue + if target_row_edge_id in node_id: + continue + if self._is_waypoint_name(node_id): + continue + + node_obj = self.route_search.get_node_from_tmap2(node_id) + if not node_obj or "node" not in node_obj or "pose" not in node_obj["node"]: + continue + + pose = node_obj["node"]["pose"] + xy = np.array([pose["position"]["x"], pose["position"]["y"]], dtype=float) + candidates[node_id] = (pose, xy) + + return candidates + + def _select_boundary_nodes(self, candidates, center_xy, row_dir, tag_id): + """ + Select left and right boundary nodes relative to the row direction. + Returns: list of selected node_ids (up to 2). + """ + scored = [] # (side_sign, dist, node_id) + + for node_id, (_pose, txy) in candidates.items(): + if tag_id not in node_id: + continue + + v = txy - center_xy + dist = float(np.linalg.norm(v)) + if dist < 1e-6: + continue + + # Reject nodes too aligned with row direction (want perpendicular/lateral nodes) + cosang = abs(float(np.dot(row_dir, v) / dist)) + if cosang > 0.6: + continue + + # Determine which side (left=positive, right=negative) + crossz = float(row_dir[0] * v[1] - row_dir[1] * v[0]) + side = 1.0 if crossz > 0.0 else -1.0 + scored.append((side, dist, node_id)) + + if not scored: + return [] + + # Pick closest on each side + left = min((c for c in scored if c[0] > 0.0), default=None, key=lambda x: x[1]) + right = min((c for c in scored if c[0] < 0.0), default=None, key=lambda x: x[1]) + + picked = [] + if left: + picked.append(left[2]) + if right: + picked.append(right[2]) + + # Fallback: fill up to 2 from remaining candidates + if len(picked) < 2: + for _, _, nid in sorted(scored, key=lambda x: x[1]): + if nid not in picked: + picked.append(nid) + if len(picked) == 2: + break + + return picked + + def _select_last_row_goal(self, nodes): + """Select the last goal that is a ROW node (not a waypoint).""" + selected = None + for g in nodes: + try: + p = g["target_pose"]["pose"]["position"] + xy = (p["x"], p["y"]) + name = getattr(self, "destination_node_str", {}).get(xy) + if name and self._is_row_node_name(name): + selected = g + except Exception: + continue + return selected if selected else nodes[-1] + + def _handle_row_operation(self, nodes, edge_id, action_msg): + """ + Build row boundaries and return (action, action_msg). + Simplified logic for selecting boundary nodes. + """ + # Validate inputs + if not nodes: + self.get_logger().error("[_handle_row_operation] nodes list is empty") + self._publish_empty_boundary("map") + action_msg.setSideEdges({}, "map") + return self.ACTIONS.ROW_OPERATION, action_msg + + frame_id = nodes[0]["target_pose"]["header"].get("frame_id", "map") + + if not edge_id: + self.get_logger().error("[_handle_row_operation] edge_id is empty") + self._publish_empty_boundary(frame_id) + action_msg.setSideEdges({}, frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg + + # Get row center node + cen, tag_id, target_row_edge_id = self._get_row_center_node(edge_id) + if not cen: + self._publish_empty_boundary(frame_id) + action_msg.setSideEdges({}, frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg + + self.get_logger().info(f"[_handle_row_operation] center={target_row_edge_id}, tag={tag_id}") + # get target_row_edge_id pose and orientation + target_row_edge_node = self.route_search.get_node_from_tmap2(target_row_edge_id) + if target_row_edge_node and "node" in target_row_edge_node: + target_row_edge_pose = target_row_edge_node["node"]["pose"] + self.get_logger().info(f"Target Row Edge Pose: {target_row_edge_pose}") + else: + self.get_logger().warn(f"[_handle_row_operation] Could not find node for target_row_edge_id: {target_row_edge_id}") + + try: + # print out cen info + # self.get_logger().info(f"Center Node Info: {cen}") + # get target edges from cen and publish as pose stamped message + node_info = cen.get("node", {}) + node_pose = node_info.get("pose", {}) + parent_frame = node_info.get("parent_frame", "map") + # get the node IDs from cen edges + selected_edges_dict = {"node": {"parent_frame": parent_frame, "pose": node_pose, "edges": []}} + # print node pose + self.get_logger().info(f"Center Node Pose: {node_pose}") + + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'map' + msg.pose.position.x = node_pose["position"]["x"] + msg.pose.position.y = node_pose["position"]["y"] + msg.pose.position.z = node_pose["position"]["z"] + msg.pose.orientation.x = node_pose["orientation"]["x"] + msg.pose.orientation.y = node_pose["orientation"]["y"] + msg.pose.orientation.z = node_pose["orientation"]["z"] + msg.pose.orientation.w = node_pose["orientation"]["w"] + self.center_node_pose_pub.publish(msg) + self.get_logger().info('Published latched Center Node Pose') + + except Exception as e: + self.get_logger().error(f"[_handle_row_operation] Failed to extract center node info: {e}") + + # Collect boundary candidates + candidates = self._collect_boundary_candidates(cen, target_row_edge_id) + if not candidates: + self.get_logger().error("[_handle_row_operation] No boundary candidates found") + self._publish_empty_boundary(frame_id) + action_msg.setSideEdges({}, frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg + + # Select last row goal and compute row direction + selected_last_node = self._select_last_row_goal(nodes) + cpos = cen["node"]["pose"]["position"] + lpos = selected_last_node["target_pose"]["pose"]["position"] + + # Avoid using center as last goal + if np.linalg.norm(np.array([lpos["x"] - cpos["x"], lpos["y"] - cpos["y"]])) < 1e-6: + selected_last_node = nodes[0] + lpos = selected_last_node["target_pose"]["pose"]["position"] + + dx, dy = lpos["x"] - cpos["x"], lpos["y"] - cpos["y"] + row_yaw = math.atan2(dy, dx) if abs(dx) + abs(dy) > 1e-6 else self._yaw_from_quat(cen["node"]["pose"]["orientation"]) + row_dir = np.array([math.cos(row_yaw), math.sin(row_yaw)], dtype=float) + center_xy = np.array([cpos["x"], cpos["y"]], dtype=float) + + # Select boundary nodes + picked = self._select_boundary_nodes(candidates, center_xy, row_dir, tag_id) + if not picked: + self.get_logger().error("[_handle_row_operation] No valid boundary nodes selected") + self._publish_empty_boundary(frame_id) + action_msg.setSideEdges({}, frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg + + # Build selected_edges with poses aligned to row direction + self.selected_edges = {} + for child in picked: + child_node = self.route_search.get_node_from_tmap2(child) + if not child_node or "node" not in child_node: + continue + + side_pose = self.get_intermediate_pose(cen, child_node, frame_id) + side_last_pose = self.get_last_intermediate_pose(side_pose, cen, selected_last_node) + + # self.get_logger().info(f"Side Pose: {side_pose}") + self.get_logger().info(f"Side Last Pose: {side_last_pose}") + self.get_logger().info(f"Row Yaw: {row_yaw}") + self.get_logger().info(f"Child Node ID: {child}") + # how to print the node orientation?? + self.get_logger().info(f"Child Node Orientation: {child_node['node']['pose']['orientation']}") + # print node orientation as yaw + child_yaw = self._yaw_from_quat(child_node["node"]["pose"]["orientation"]) + self.get_logger().info(f"Child Node Yaw: {child_yaw}") + + self._set_pose_yaw(side_pose, child_yaw) + self._set_pose_yaw(side_last_pose, child_yaw) + self.selected_edges[child] = [side_pose, side_last_pose] + + # Mirror if only one wall found + if len(self.selected_edges) == 1: + self.get_logger().info("[_handle_row_operation] Single wall found, creating mirror") + self.selected_edges["side_wall"] = self.get_intermediate_poses_interpolated( + self.selected_edges, cen, selected_last_node + ) + for ps in self.selected_edges["side_wall"]: + self._set_pose_yaw(ps, child_yaw) + + if not self.selected_edges: + self.get_logger().error("[_handle_row_operation] No edges built") + self._publish_empty_boundary(frame_id) + action_msg.setSideEdges({}, frame_id) + return self.ACTIONS.ROW_OPERATION, action_msg + + # Publish boundary + action_msg.setSideEdges(self.selected_edges, frame_id) + if not self.is_row_boundary_published: + boundary_info = action_msg.getBoundary() + boundary_info.header.stamp = self.get_clock().now().to_msg() + self.boundary_publisher.publish(boundary_info) + self.is_row_boundary_published = True + + return self.ACTIONS.ROW_OPERATION, action_msg + + def get_navigate_through_poses_goal(self, poses, actions, edge_ids, is_execpolicy=False): + + control_server_configs = {} action_msgs = [] @@ -645,9 +929,9 @@ def handle_row_operation(): nav_goal.behavior_tree = self.bt_trees[action] edge_action_is_valid = True - if(action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation == True and (len(nodes) > 0)): - self.get_logger().warn(f"Segment {seg_i} action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation == True and (len(nodes) > 0)") - action, action_msg = handle_row_operation() + if action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation and nodes: + self.get_logger().warn(f"Segment {seg_i}: ROW_TRAVERSAL with in_row_operation") + action, action_msg = self._handle_row_operation(nodes, edge_id, action_msg) self.get_logger().info(" Action {} Bt_tree : {}".format(action, nav_goal.behavior_tree)) if edge_action_is_valid: @@ -678,12 +962,13 @@ def handle_row_operation(): if action in self.ACTIONS.bt_tree_with_control_server_config: controller_plugin = self.ACTIONS.bt_tree_with_control_server_config[action] control_server_configs[action] = self.ACTIONS.planner_with_goal_checker_config[controller_plugin] - if(action in self.bt_trees): + if action in self.bt_trees: nav_goal.behavior_tree = self.bt_trees[action] edge_action_is_valid = True - if(action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation == True and (len(nodes) > 0)): - action, action_msg = handle_row_operation() + if action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation and nodes: + self.get_logger().warn(f"Segment {seg_i}: ROW_TRAVERSAL with in_row_operation") + action, action_msg = self._handle_row_operation(nodes, edge_id, action_msg) self.get_logger().info(" Action {} Bt_tree : {}".format(action, nav_goal.behavior_tree)) if edge_action_is_valid: diff --git a/topological_navigation/topological_navigation/manager2.py b/topological_navigation/topological_navigation/manager2.py index 49795cea..d0b11a83 100644 --- a/topological_navigation/topological_navigation/manager2.py +++ b/topological_navigation/topological_navigation/manager2.py @@ -48,7 +48,7 @@ def construct_mapping(self, node, deep=False): # this can be extended to test the validity of the tmap2 # as well at load time (or add missing keys) - for key in ['x', 'y', 'z', 'w']: + for key in ['x', 'y', 'z', 'w', 'yaw_goal_tolerance', 'xy_goal_tolerance']: if key in mapping and isinstance(mapping[key], int): mapping[key] = float(mapping[key]) diff --git a/topological_navigation/topological_navigation/scripts/navigation2.py b/topological_navigation/topological_navigation/scripts/navigation2.py index 166dc29a..0bef2339 100755 --- a/topological_navigation/topological_navigation/scripts/navigation2.py +++ b/topological_navigation/topological_navigation/scripts/navigation2.py @@ -7,7 +7,7 @@ import rclpy, json, yaml - +import math from topological_navigation_msgs.msg import NavStatistics, CurrentEdge, ClosestEdges, TopologicalRoute, GotoNodeFeedback, ExecutePolicyModeFeedback from topological_navigation_msgs.srv import EvaluateEdge, EvaluateNode from topological_navigation_msgs.action import GotoNode, ExecutePolicyMode @@ -19,7 +19,7 @@ from topological_navigation.navigation_stats import nav_stats from topological_navigation.scripts.param_processing import ParameterUpdaterNode from topological_navigation.tmap_utils import * -from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy, QoSDurabilityPolicy +from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy from rclpy.action import ActionServer from rclpy import Parameter from topological_navigation.edge_action_manager2 import EdgeActionManager @@ -75,6 +75,9 @@ def __init__(self, name, update_params_control_server, edge_action_manager_serve self.fluid_navigation = True self.final_goal = False self.update_params_control_server = update_params_control_server + + self.route = None + self.target = None self.current_node = "Unknown" self.closest_node = "Unknown" @@ -113,6 +116,9 @@ def __init__(self, name, update_params_control_server, edge_action_manager_serve self.declare_parameter(self.ACTIONS.BT_GOAL_ALIGN, Parameter.Type.STRING) self.declare_parameter(self.ACTIONS.BT_IN_ROW_OPERATION, Parameter.Type.STRING) self.declare_parameter(self.ACTIONS.BT_IN_ROW_RECOVERY, Parameter.Type.STRING) + + self.declare_parameter("allow_intermediate_orientation_override", Parameter.Type.BOOL) + self.allow_intermediate_orientation_override = self.get_parameter_or("allow_intermediate_orientation_override", Parameter('bool', Parameter.Type.BOOL, False)).value self.navigation_action_name = self.get_parameter_or("navigation_action_name", Parameter('str', Parameter.Type.STRING, self.ACTIONS.NAVIGATE_TO_POSE)).value self.navigation_actions = self.get_parameter_or("navigation_actions", Parameter('str', Parameter.Type.STRING_ARRAY, self.ACTIONS.navigation_actions)).value @@ -168,9 +174,12 @@ def __init__(self, name, update_params_control_server, edge_action_manager_serve self.navigation_actions.append(self.navigation_action_name) self.latching_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self.keep_history_qos = QoSProfile(reliability = ReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.KEEP_LAST, depth=10, ) self.stat = None self.stats_pub = self.create_publisher(NavStatistics, "topological_navigation/Statistics", qos_profile=self.latching_qos) - self.route_pub = self.create_publisher(TopologicalRoute, "topological_navigation/Route", qos_profile=self.latching_qos) + self.route_pub = self.create_publisher(TopologicalRoute, "topological_navigation/Route", qos_profile= self.keep_history_qos) + self.route_pub_timer = self.create_timer( 2.0 , self.router_pub_timer_callback) + self.stroute = None self.cur_edge = self.create_publisher(String, "current_edge", qos_profile=self.latching_qos) self.move_act_pub = self.create_publisher(String, "topological_navigation/move_action_status", qos_profile=self.latching_qos) self._map_received = False @@ -746,6 +755,50 @@ def navigate_to_poses(self, route, target, exec_policy): rindex = rindex + 1 self.get_logger().info(" ========== Action list {} ".format(route_actions_list)) + + if self.allow_intermediate_orientation_override: + # ====================================================================== + # ## NEW LOGIC START: Realign Orientations for Continuous Flow + # ====================================================================== + # We iterate through all destinations except the very last one. + # We point each node to look at the NEXT node. + + for i in range(len(route_dests) - 1): + curr_node = route_dests[i] + next_node = route_dests[i+1] + + # 1. Get positions + # Note: Adjust keys ["pose"]["position"] if your dictionary structure is different + cx = curr_node["node"]["pose"]["position"]["x"] + cy = curr_node["node"]["pose"]["position"]["y"] + nx = next_node["node"]["pose"]["position"]["x"] + ny = next_node["node"]["pose"]["position"]["y"] + + # 2. Calculate the specific angle (Yaw) to the next node + dx = nx - cx + dy = ny - cy + yaw = math.atan2(dy, dx) + + # 3. Convert Yaw to Quaternion manually (to avoid extra dependencies) + # Formula for Z-axis rotation + qz = math.sin(yaw * 0.5) + qw = math.cos(yaw * 0.5) + + # 4. Overwrite the orientation of the intermediate node + # Now the planner thinks this node is "facing" the path, so it won't stop to turn. + curr_node["node"]["pose"]["orientation"]["x"] = 0.0 + curr_node["node"]["pose"]["orientation"]["y"] = 0.0 + curr_node["node"]["pose"]["orientation"]["z"] = qz + curr_node["node"]["pose"]["orientation"]["w"] = qw + + self.get_logger().info(f"Realigned node {i} to yaw: {yaw:.2f}") + + # The LAST node (route_dests[-1]) is left untouched so it keeps + # the final desired docking/goal orientation. + # ====================================================================== + # ## NEW LOGIC END + # ====================================================================== + nav_ok, inc, status = self.execute_actions(route_edges, route_dests, route_origins, action_name=self.ACTIONS.NAVIGATE_THROUGH_POSES, is_execpolicy=exec_policy) @@ -792,6 +845,7 @@ def navigate(self, target): self.max_dist_to_closest_edge = self.get_parameter_or("max_dist_to_closest_edge", Parameter('double', Parameter.Type.DOUBLE, 1.0)).value + # if we are nowhere near an edge or not at a node, then do a node plan if self.closest_edges.distances and (self.closest_edges.distances[0] > self.max_dist_to_closest_edge or self.current_node != "none"): self.nav_from_closest_edge = False o_node = self.rsearch.get_node_from_tmap2(self.closest_node) @@ -799,6 +853,9 @@ def navigate(self, target): else: self.nav_from_closest_edge = True o_node, the_edge = self.orig_node_from_closest_edge(g_node) + # This creates essentially a fake "previous node" to address the edge case when navigating over a single edge and the closest node is on the edge but the current node isnt. (otherwise it will mark as complete without navigation). + if o_node == target: + o_node, the_edge = self.orig_node_from_closest_edge(g_node, flip=True) self.get_logger().info("Planning from the closest EDGE: {}".format(the_edge["edge_id"])) self.get_logger().info("Navigating From Origin {} to Target {} ".format(o_node["node"]["name"], target)) @@ -811,7 +868,8 @@ def navigate(self, target): route = self.enforce_navigable_route(route, target) if route.source: self.get_logger().info("Navigating Case 1: Following route") - self.get_logger().warn("[navigate] - publishing route") + self.route = route + self.target = target self.publish_route(route, target) if(self.use_nav2_follow_route): result, inc, status = self.navigate_to_poses(route, target, 0) @@ -891,7 +949,7 @@ def publish_feedback_exec_policy(self, nav_outcome=None): self._as_exec_policy_action_feedback_pub.publish(self._feedback_exec_policy) - def orig_node_from_closest_edge(self, g_node): + def orig_node_from_closest_edge(self, g_node, flip=False): name_1, _ = get_node_names_from_edge_id_2(self.lnodes, self.closest_edges.edge_ids[0]) name_2, _ = get_node_names_from_edge_id_2(self.lnodes, self.closest_edges.edge_ids[1]) @@ -911,11 +969,18 @@ def orig_node_from_closest_edge(self, g_node): d2 = get_route_distance(self.lnodes, o_node_2, g_node) else: # Use the destination node of the closest edge. d1 = 0; d2 = 1 - if d1 <= d2: - return o_node_1, edge_1 + if flip: + if d1 <= d2: + return o_node_1, edge_1 + else: + return o_node_2, edge_2 else: - return o_node_2, edge_2 - + o_node_1b = self.rsearch.get_node_from_tmap2(name_1) + o_node_2b = self.rsearch.get_node_from_tmap2(name_2) + if d1 <= d2: + return o_node_1b, edge_1 + else: + return o_node_2b, edge_2 def to_goal_node(self, g_node, the_edge=None): self.get_logger().info("Target and Origin Nodes are the same") @@ -1010,8 +1075,13 @@ def publish_route(self, route, target): for i in route.source: stroute.nodes.append(i) stroute.nodes.append(target) + self.stroute = stroute self.route_pub.publish(stroute) + + def router_pub_timer_callback(self): + if (self.stroute is not None and self.stroute.nodes): + self.route_pub.publish(self.stroute) def publish_stats(self): pubst = NavStatistics() @@ -1257,4 +1327,4 @@ def main(): rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/topological_navigation/topological_navigation/scripts/validate_map.py b/topological_navigation/topological_navigation/scripts/validate_map.py new file mode 100644 index 00000000..9a94b213 --- /dev/null +++ b/topological_navigation/topological_navigation/scripts/validate_map.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python3 +""" +Topological Map Schema Validator + +This script validates topological map YAML files against the tmap-schema.yaml schema. +It can be used standalone (without ROS) to verify map files are correctly formatted. + +Usage: + python3 validate_map.py [schema_file.yaml] + +If schema_file is not provided, the script will look for tmap-schema.yaml in the +standard config directory. + +Exit codes: + 0 - Validation successful + 1 - Validation failed + 2 - File not found or other error +""" + +import sys +import os +import argparse + +try: + import yaml +except ImportError: + print("Error: PyYAML is required. Install with: pip install pyyaml") + sys.exit(2) + +try: + import jsonschema +except ImportError: + print("Error: jsonschema is required. Install with: pip install jsonschema") + sys.exit(2) + + +def find_schema_file(): + """Find the schema file in standard locations.""" + # Try relative to this script + script_dir = os.path.dirname(os.path.abspath(__file__)) + + # Possible locations for the schema file + possible_paths = [ + os.path.join(script_dir, '..', '..', 'config', 'tmap-schema.yaml'), + os.path.join(script_dir, '..', 'config', 'tmap-schema.yaml'), + os.path.join(script_dir, 'config', 'tmap-schema.yaml'), + ] + + # Try ROS2 package share directory if ament_index is available + try: + from ament_index_python.packages import get_package_share_directory + package_path = get_package_share_directory('topological_navigation') + possible_paths.insert(0, os.path.join(package_path, 'config', 'tmap-schema.yaml')) + except (ImportError, Exception): + pass + + for path in possible_paths: + normalized_path = os.path.normpath(path) + if os.path.isfile(normalized_path): + return normalized_path + + return None + + +def load_yaml_file(filepath): + """Load a YAML file and return its contents.""" + try: + with open(filepath, 'r', encoding='utf-8') as f: + return yaml.safe_load(f) + except yaml.YAMLError as e: + raise ValueError(f"Invalid YAML syntax: {e}") + except FileNotFoundError: + raise FileNotFoundError(f"File not found: {filepath}") + except Exception as e: + raise RuntimeError(f"Error reading file: {e}") + + +def validate_map(map_file, schema_file=None, verbose=False): + """ + Validate a topological map file against the schema. + + Args: + map_file: Path to the topological map YAML file + schema_file: Path to the schema YAML file (optional) + verbose: Print detailed validation information + + Returns: + Tuple of (is_valid: bool, message: str) + """ + # Find schema file if not provided + if schema_file is None: + schema_file = find_schema_file() + if schema_file is None: + return False, "Could not find tmap-schema.yaml. Please specify schema file path." + + if verbose: + print(f"Using schema: {schema_file}") + print(f"Validating map: {map_file}") + + # Load schema + try: + schema = load_yaml_file(schema_file) + except Exception as e: + return False, f"Error loading schema: {e}" + + # Load map + try: + tmap = load_yaml_file(map_file) + except Exception as e: + return False, f"Error loading map: {e}" + + # Validate + try: + jsonschema.validate(tmap, schema) + + # Additional validation checks + warnings = [] + + # Check for node count + node_count = len(tmap.get('nodes', [])) + if verbose: + print(f"Found {node_count} nodes") + + # Check for duplicate node names + node_names = set() + node_names_list = [] # Keep a list for edge validation + for node_entry in tmap.get('nodes', []): + node = node_entry.get('node', {}) + name = node.get('name') + if name: + if name in node_names: + warnings.append(f"Duplicate node name: {name}") + node_names.add(name) + node_names_list.append(name) + + # Check for edges pointing to non-existent nodes + for node_entry in tmap.get('nodes', []): + node = node_entry.get('node', {}) + for edge in node.get('edges', []): + target = edge.get('node') + if target and target not in node_names: + warnings.append(f"Edge '{edge.get('edge_id')}' points to non-existent node: {target}") + + message = "Validation successful" + if warnings: + message += f"\n\nWarnings ({len(warnings)}):\n" + "\n".join(f" - {w}" for w in warnings) + + return True, message + + except jsonschema.ValidationError as e: + # Format a user-friendly error message + path = " -> ".join(str(p) for p in e.absolute_path) if e.absolute_path else "root" + return False, f"Validation failed at '{path}':\n {e.message}" + except jsonschema.SchemaError as e: + return False, f"Schema error: {e.message}" + + +def main(): + parser = argparse.ArgumentParser( + description='Validate topological map YAML files against the schema.', + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog=""" +Examples: + %(prog)s my_map.tmap2.yaml + %(prog)s my_map.tmap2.yaml --schema custom_schema.yaml + %(prog)s my_map.tmap2.yaml -v + """ + ) + parser.add_argument('map_file', help='Path to the topological map YAML file') + parser.add_argument('--schema', '-s', help='Path to the schema YAML file (optional)') + parser.add_argument('--verbose', '-v', action='store_true', help='Print detailed information') + + args = parser.parse_args() + + # Check if map file exists + if not os.path.isfile(args.map_file): + print(f"Error: Map file not found: {args.map_file}") + sys.exit(2) + + # Validate + is_valid, message = validate_map(args.map_file, args.schema, args.verbose) + + if is_valid: + print(f"✓ {message}") + sys.exit(0) + else: + print(f"✗ {message}") + sys.exit(1) + + +if __name__ == '__main__': + main() diff --git a/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py b/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py index f54dd00b..e719ad6a 100755 --- a/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py +++ b/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py @@ -24,6 +24,7 @@ from rclpy.task import Future import threading import yaml +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy # this ensures that all the poses and translates # are float-type and not int-type as there is an