diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 74d19e93..8269b0d6 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -1,26 +1,35 @@ -FROM ros:humble +ARG BASE_IMAGE=lcas.lincoln.ac.uk/ros:humble-staging +ARG ROS_DISTRO=humble + +FROM ${BASE_IMAGE} AS devcontainer RUN rm -rf /var/lib/apt/lists/* \ && apt-get update --fix-missing \ - && export DEBIAN_FRONTEND=noninteractive \ && apt-get -y install --no-install-recommends --fix-missing \ - ros-humble-desktop \ - bash-completion \ - python3-colcon-common-extensions \ - python3-pip \ - python3-vcstool \ - python3-rosdep \ - nano \ - git \ + ros-${ROS_DISTRO}-rviz2 \ + bash-completion \ + python3-colcon-common-extensions \ + python3-pip \ + python3-rosdep \ && rm -rf /var/lib/apt/lists/* -RUN useradd -rm -d /home/lcas -s /bin/bash -g root -G sudo -u 1001 lcas -RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers +COPY ./ /workspace +WORKDIR /workspace +ENV ROS2_WS=/workspace + +RUN sudo rm -rf log/ build/ install/ + +RUN rosdep update --rosdistro ${ROS_DISTRO} &&\ + sudo apt-get update && \ + rosdep install --from-paths ./src --ignore-src -r -y + +# Do an initial build to speed up development +RUN /bin/bash -lc "source /opt/ros/${ROS_DISTRO}/setup.bash && colcon build --continue-on-error" -RUN echo "source /opt/ros/humble/setup.bash" >> /home/lcas/.bashrc +# ---------------------------------------------------------------------------------------------------- +FROM devcontainer AS final -COPY *.sh /tmp/.devcontainer/ -RUN bash /tmp/.devcontainer/install.sh +RUN /bin/bash -lc "source /opt/ros/${ROS_DISTRO}/setup.bash && colcon build" -RUN mkdir -p /home/lcas/ws/src && ln -s /workspaces /home/lcas/ws/src/workspaces -WORKDIR /home/lcas/ws +ENV MAP_FILE=/home/ros/workspace/src/topological_navigation/config/test_simple_tmap2.yaml +CMD ["/bin/bash", "-lc", "source /workspace/install/setup.bash && exec ros2 launch topological_navigation topological_navigation.launch.py map_path:=$MAP_FILE"] diff --git a/.devcontainer/compose.yaml b/.devcontainer/compose.yaml new file mode 100644 index 00000000..a2925966 --- /dev/null +++ b/.devcontainer/compose.yaml @@ -0,0 +1,42 @@ +services: + vnc: + image: lcas.lincoln.ac.uk/vnc + volumes: + - x11:/tmp/.X11-unix + devices: + - /dev/dri:/dev/dri + networks: + - ros_net + shm_size: '2gb' + stdin_open: true + tty: true + + topo-nav: + build: + context: ../ + dockerfile: .devcontainer/Dockerfile + target: devcontainer + volumes: + - x11:/tmp/.X11-unix + - ../:/home/ros/workspace + - colcon_build:/home/ros/workspace/build + - colcon_install:/home/ros/workspace/install + - colcon_log:/home/ros/workspace/log + stdin_open: true + tty: true + networks: + - ros_net + depends_on: + - vnc + environment: + - DISPLAY=:1.0 + - ROS_DOMAIN_ID=10 + +volumes: + x11: + colcon_build: + colcon_install: + colcon_log: + +networks: + ros_net: diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 6ae27733..56da8ac8 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,27 +1,16 @@ { "name": "topological_navigation", - "build": { - "dockerfile": "Dockerfile", - "context": "." - }, - - "features": { - "ghcr.io/devcontainers/features/desktop-lite:1": {} - }, - - "forwardPorts": [6080], + "dockerComposeFile": "compose.yaml", + "service": "topo-nav", + "workspaceFolder": "/home/ros/workspace", + "containerUser": "ros", + "remoteUser": "ros", + "updateRemoteUserUID": true, + + "forwardPorts": ["vnc:5801"], "portsAttributes": { - "6080": { "label": "novnc" } - }, - - "postCreateCommand": "bash .devcontainer/run.sh", - "containerUser": "lcas", - "runArgs": ["--security-opt", "seccomp=unconfined"], - - "containerEnv": { - "ROS_LOCALHOST_ONLY": "0", - "LIBGL_ALWAYS_SOFTWARE": "1" - }, + "vnc:5801": { "label": "novnc" } + }, "customizations": { "vscode": { diff --git a/.devcontainer/install.sh b/.devcontainer/install.sh deleted file mode 100644 index 4177d81e..00000000 --- a/.devcontainer/install.sh +++ /dev/null @@ -1,11 +0,0 @@ -#!/bin/bash - -set -e - -source /opt/ros/humble/setup.bash -apt-get update -rosdep init || true -rosdep --rosdistro=humble update - -rm -rf /var/lib/apt/lists/* - diff --git a/.devcontainer/run.sh b/.devcontainer/run.sh deleted file mode 100644 index 94104a10..00000000 --- a/.devcontainer/run.sh +++ /dev/null @@ -1,23 +0,0 @@ -#!/bin/bash - -set -e - -source /opt/ros/humble/setup.bash - -# Only run apt/rosdep update if not done before (avoid hanging on rebuild) -MARKER="/home/lcas/.postCreateDone" -if [ ! -f "$MARKER" ]; then - sudo apt-get update -qq || true - rosdep --rosdistro=humble update --include-eol-distros || true - cd /home/lcas/ws - rosdep install --from-paths ./src -i -y --rosdistro=humble || true - touch "$MARKER" -fi - -cd /home/lcas/ws -colcon build --symlink-install - -# Add workspace sourcing to bashrc (idempotent) -grep -q "install/setup.bash" ~/.bashrc 2>/dev/null || \ - echo "source /home/lcas/ws/install/setup.bash" >> ~/.bashrc - diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 434f75db..5026d54f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -6,7 +6,6 @@ on: tags: # Also run CI on tags. - '*' pull_request: - branches: [ humble-dev, aoc ] jobs: # ------------------------------------------------------------------- @@ -32,7 +31,7 @@ jobs: pip install pytest pyyaml jsonschema networkx scipy numpy - name: Run topological_navigation unit tests - working-directory: topological_navigation + working-directory: src/topological_navigation run: | python -m pytest test/ \ --ignore=test/test_navigationcore.py \ @@ -40,7 +39,7 @@ jobs: -v --tb=short - name: Run topological_nav_simulator unit tests - working-directory: topological_nav_simulator + working-directory: src/topological_nav_simulator run: | python -m pytest test/ -v --tb=short || true # These tests require geometry_msgs (ROS 2); they are @@ -50,11 +49,13 @@ jobs: # Full ROS 2 build + colcon test # ------------------------------------------------------------------- test_docker: + name: ROS Testing ${{ matrix.ros_distribution }} runs-on: ubuntu-latest strategy: matrix: ros_distribution: - humble + - jazzy include: # Humble Hawksbill (May 2022 - May 2027) @@ -62,15 +63,20 @@ jobs: ros_distribution: humble ros_version: 2 + # Jazzy Jalisco (May 2024 - May 2029) + - docker_image: ubuntu:noble + ros_distribution: jazzy + ros_version: 2 + container: image: ${{ matrix.docker_image }} steps: - name: setup ROS environment - uses: LCAS/setup-ros@master + uses: ros-tooling/setup-ros@main with: required-ros-distributions: ${{ matrix.ros_distribution }} - name: build and test ROS 2 if: ${{ matrix.ros_version == 2 }} uses: ros-tooling/action-ros-ci@v0.3 with: - target-ros2-distro: ${{ matrix.ros_distribution }} \ No newline at end of file + target-ros2-distro: ${{ matrix.ros_distribution }} diff --git a/demo.compose.yaml b/demo.compose.yaml new file mode 100644 index 00000000..7cc2b65d --- /dev/null +++ b/demo.compose.yaml @@ -0,0 +1,39 @@ +services: + vnc: + image: lcas.lincoln.ac.uk/vnc:staging + ports: + - "5801:5801" + volumes: + - x11:/tmp/.X11-unix + devices: + - /dev/dri:/dev/dri + networks: + - ros_net + shm_size: '2gb' + stdin_open: true + tty: true + + topo-nav: + build: + context: . + dockerfile: .devcontainer/Dockerfile + target: final + volumes: + - x11:/tmp/.X11-unix + stdin_open: true + tty: true + networks: + - ros_net + depends_on: + - vnc + environment: + - DISPLAY=:1.0 + - ROS_DOMAIN_ID=10 + - MAP_FILE=/home/ros/workspace/src/topological_navigation/config/test_complex_tmap2.yaml + - FAKE_ROBOT=true + +volumes: + x11: + +networks: + ros_net: \ No newline at end of file diff --git a/topological_nav_simulator/README.md b/src/topological_nav_simulator/README.md similarity index 100% rename from topological_nav_simulator/README.md rename to src/topological_nav_simulator/README.md diff --git a/topological_nav_simulator/config/fake_nav2.rviz b/src/topological_nav_simulator/config/fake_nav2.rviz similarity index 100% rename from topological_nav_simulator/config/fake_nav2.rviz rename to src/topological_nav_simulator/config/fake_nav2.rviz diff --git a/topological_nav_simulator/launch/fake_nav2.launch.py b/src/topological_nav_simulator/launch/fake_nav2.launch.py similarity index 100% rename from topological_nav_simulator/launch/fake_nav2.launch.py rename to src/topological_nav_simulator/launch/fake_nav2.launch.py diff --git a/topological_nav_simulator/package.xml b/src/topological_nav_simulator/package.xml similarity index 100% rename from topological_nav_simulator/package.xml rename to src/topological_nav_simulator/package.xml diff --git a/topological_nav_simulator/resource/topological_nav_simulator b/src/topological_nav_simulator/resource/topological_nav_simulator similarity index 100% rename from topological_nav_simulator/resource/topological_nav_simulator rename to src/topological_nav_simulator/resource/topological_nav_simulator diff --git a/topological_nav_simulator/setup.cfg b/src/topological_nav_simulator/setup.cfg similarity index 100% rename from topological_nav_simulator/setup.cfg rename to src/topological_nav_simulator/setup.cfg diff --git a/topological_nav_simulator/setup.py b/src/topological_nav_simulator/setup.py similarity index 100% rename from topological_nav_simulator/setup.py rename to src/topological_nav_simulator/setup.py diff --git a/topological_nav_simulator/test/test_virtual_robot.py b/src/topological_nav_simulator/test/test_virtual_robot.py similarity index 100% rename from topological_nav_simulator/test/test_virtual_robot.py rename to src/topological_nav_simulator/test/test_virtual_robot.py diff --git a/topological_nav_simulator/topological_nav_simulator/__init__.py b/src/topological_nav_simulator/topological_nav_simulator/__init__.py similarity index 100% rename from topological_nav_simulator/topological_nav_simulator/__init__.py rename to src/topological_nav_simulator/topological_nav_simulator/__init__.py diff --git a/topological_nav_simulator/topological_nav_simulator/fake_nav2_server.py b/src/topological_nav_simulator/topological_nav_simulator/fake_nav2_server.py similarity index 100% rename from topological_nav_simulator/topological_nav_simulator/fake_nav2_server.py rename to src/topological_nav_simulator/topological_nav_simulator/fake_nav2_server.py diff --git a/topological_nav_simulator/topological_nav_simulator/virtual_robot.py b/src/topological_nav_simulator/topological_nav_simulator/virtual_robot.py similarity index 100% rename from topological_nav_simulator/topological_nav_simulator/virtual_robot.py rename to src/topological_nav_simulator/topological_nav_simulator/virtual_robot.py diff --git a/topological_navigation/CHANGELOG.rst b/src/topological_navigation/CHANGELOG.rst similarity index 100% rename from topological_navigation/CHANGELOG.rst rename to src/topological_navigation/CHANGELOG.rst diff --git a/topological_navigation/CMakeLists.txt b/src/topological_navigation/CMakeLists.txt similarity index 100% rename from topological_navigation/CMakeLists.txt rename to src/topological_navigation/CMakeLists.txt diff --git a/topological_navigation/LICENSE b/src/topological_navigation/LICENSE similarity index 100% rename from topological_navigation/LICENSE rename to src/topological_navigation/LICENSE diff --git a/topological_navigation/README.md b/src/topological_navigation/README.md similarity index 100% rename from topological_navigation/README.md rename to src/topological_navigation/README.md diff --git a/topological_navigation/config/template_action.yaml b/src/topological_navigation/config/template_action.yaml similarity index 100% rename from topological_navigation/config/template_action.yaml rename to src/topological_navigation/config/template_action.yaml diff --git a/topological_navigation/config/template_edge.yaml b/src/topological_navigation/config/template_edge.yaml similarity index 100% rename from topological_navigation/config/template_edge.yaml rename to src/topological_navigation/config/template_edge.yaml diff --git a/topological_navigation/config/template_node.yaml b/src/topological_navigation/config/template_node.yaml similarity index 100% rename from topological_navigation/config/template_node.yaml rename to src/topological_navigation/config/template_node.yaml diff --git a/topological_navigation/config/test_complex_tmap2.yaml b/src/topological_navigation/config/test_complex_tmap2.yaml similarity index 100% rename from topological_navigation/config/test_complex_tmap2.yaml rename to src/topological_navigation/config/test_complex_tmap2.yaml diff --git a/topological_navigation/config/test_simple_tmap2.yaml b/src/topological_navigation/config/test_simple_tmap2.yaml similarity index 100% rename from topological_navigation/config/test_simple_tmap2.yaml rename to src/topological_navigation/config/test_simple_tmap2.yaml diff --git a/topological_navigation/config/tmap-schema.yaml b/src/topological_navigation/config/tmap-schema.yaml similarity index 100% rename from topological_navigation/config/tmap-schema.yaml rename to src/topological_navigation/config/tmap-schema.yaml diff --git a/topological_navigation/doc/NAVIGATION2.md b/src/topological_navigation/doc/NAVIGATION2.md similarity index 100% rename from topological_navigation/doc/NAVIGATION2.md rename to src/topological_navigation/doc/NAVIGATION2.md diff --git a/src/topological_navigation/launch/topological_navigation.launch.py b/src/topological_navigation/launch/topological_navigation.launch.py new file mode 100644 index 00000000..53ef8774 --- /dev/null +++ b/src/topological_navigation/launch/topological_navigation.launch.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 +# Copyright (c) 2026, topological_navigation contributors +# Licensed under the MIT License. +"""Launch the full topological navigation stack with simulation. + +Launches (in order): + 1. Map Manager -- loads and publishes the topological map + 2. Localisation -- determines current/closest node + 3. Fake Nav2 simulator -- virtual robot + fake action servers + 4. Topological map visualiser -- RViz markers + 5. RViz -- with a pre-configured display layout + +Usage +----- +Default (mixed_actions_map):: + + ros2 launch topological_navigation topological_navigation.launch.py + +Custom map file:: + + ros2 launch topological_navigation topological_navigation.launch.py \ + map_path:=/absolute/path/to/my_map.tmap2.yaml +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, TimerAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + topo_nav_share = get_package_share_directory("topological_navigation") + + # --- Default paths --- + default_map = os.path.join( + topo_nav_share, + "config", + "mixed_actions_map.yaml", + ) + default_rviz = os.path.join( + topo_nav_share, + "rviz", + "topological_navigation.rviz", + ) + + return LaunchDescription( + [ + # ============================================================= + # Launch arguments + # ============================================================= + DeclareLaunchArgument( + "map_path", + default_value=default_map, + description="Absolute path to a .tmap2.yaml topological map", + ), + DeclareLaunchArgument( + "rviz_config", + default_value=default_rviz, + description="Path to the RViz config file", + ), + DeclareLaunchArgument( + "initial_x", + default_value="0.0", + description="Initial robot X position", + ), + DeclareLaunchArgument( + "initial_y", + default_value="0.0", + description="Initial robot Y position", + ), + DeclareLaunchArgument( + "initial_yaw", + default_value="0.0", + description="Initial robot yaw (radians)", + ), + DeclareLaunchArgument( + "fake_robot", + default_value="true", + description="Use the fake Nav2 simulator", + ), + # ============================================================= + # 1. Map Manager + # ============================================================= + Node( + package="topological_navigation", + executable="map_manager2.py", + name="topological_map_manager_2", + output="screen", + arguments=[LaunchConfiguration("map_path")], + ), + # ============================================================= + # 2. Localisation (delayed 2 s so map is published first) + # ============================================================= + TimerAction( + period=2.0, + actions=[ + Node( + package="topological_navigation", + executable="localisation2.py", + name="topological_localisation", + output="screen", + ), + ], + ), + # ============================================================= + # 3. Fake Nav2 simulator (delayed 2 s, conditional) + # ============================================================= + TimerAction( + period=2.0, + condition=IfCondition(LaunchConfiguration("fake_robot")), + actions=[ + Node( + package="topological_nav_simulator", + executable="fake_nav2_server", + name="fake_nav2_server", + output="screen", + parameters=[ + { + "initial_x": LaunchConfiguration("initial_x"), + "initial_y": LaunchConfiguration("initial_y"), + "initial_yaw": LaunchConfiguration("initial_yaw"), + } + ], + ), + ], + ), + # ============================================================= + # 4. Topological navigation server (delayed 3 s) + # ============================================================= + TimerAction( + period=3.0, + actions=[ + Node( + package="topological_navigation", + executable="navigation2.py", + name="topological_navigation", + output="screen", + ), + ], + ), + # ============================================================= + # 5. Topological map visualiser (delayed 4 s) + # ============================================================= + TimerAction( + period=4.0, + actions=[ + Node( + package="topological_navigation_visual", + executable="topological_map_visualiser.py", + name="topological_map_visualiser", + output="screen", + parameters=[ + { + "edit_mode": True, + } + ], + ), + ], + ), + # ============================================================= + # 6. RViz (delayed 5 s) + # ============================================================= + TimerAction( + period=5.0, + actions=[ + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=[ + "-d", + LaunchConfiguration("rviz_config"), + ], + ), + ], + ), + ] + ) diff --git a/topological_navigation/package.xml b/src/topological_navigation/package.xml similarity index 100% rename from topological_navigation/package.xml rename to src/topological_navigation/package.xml diff --git a/topological_navigation/resource/topological_navigation b/src/topological_navigation/resource/topological_navigation similarity index 100% rename from topological_navigation/resource/topological_navigation rename to src/topological_navigation/resource/topological_navigation diff --git a/topological_navigation/rviz/topological_navigation.rviz b/src/topological_navigation/rviz/topological_navigation.rviz similarity index 100% rename from topological_navigation/rviz/topological_navigation.rviz rename to src/topological_navigation/rviz/topological_navigation.rviz diff --git a/topological_navigation/setup.cfg b/src/topological_navigation/setup.cfg similarity index 100% rename from topological_navigation/setup.cfg rename to src/topological_navigation/setup.cfg diff --git a/topological_navigation/setup.py b/src/topological_navigation/setup.py similarity index 100% rename from topological_navigation/setup.py rename to src/topological_navigation/setup.py diff --git a/topological_navigation/test/__init__.py b/src/topological_navigation/test/__init__.py similarity index 100% rename from topological_navigation/test/__init__.py rename to src/topological_navigation/test/__init__.py diff --git a/topological_navigation/test/conftest.py b/src/topological_navigation/test/conftest.py similarity index 100% rename from topological_navigation/test/conftest.py rename to src/topological_navigation/test/conftest.py diff --git a/topological_navigation/test/fixtures/README.md b/src/topological_navigation/test/fixtures/README.md similarity index 100% rename from topological_navigation/test/fixtures/README.md rename to src/topological_navigation/test/fixtures/README.md diff --git a/topological_navigation/test/fixtures/complex_map.yaml b/src/topological_navigation/test/fixtures/complex_map.yaml similarity index 100% rename from topological_navigation/test/fixtures/complex_map.yaml rename to src/topological_navigation/test/fixtures/complex_map.yaml diff --git a/topological_navigation/test/fixtures/mixed_actions_map.yaml b/src/topological_navigation/test/fixtures/mixed_actions_map.yaml similarity index 100% rename from topological_navigation/test/fixtures/mixed_actions_map.yaml rename to src/topological_navigation/test/fixtures/mixed_actions_map.yaml diff --git a/topological_navigation/test/fixtures/polygon_shapes_map.yaml b/src/topological_navigation/test/fixtures/polygon_shapes_map.yaml similarity index 100% rename from topological_navigation/test/fixtures/polygon_shapes_map.yaml rename to src/topological_navigation/test/fixtures/polygon_shapes_map.yaml diff --git a/topological_navigation/test/fixtures/simple_map.yaml b/src/topological_navigation/test/fixtures/simple_map.yaml similarity index 100% rename from topological_navigation/test/fixtures/simple_map.yaml rename to src/topological_navigation/test/fixtures/simple_map.yaml diff --git a/topological_navigation/test/static_transformer.py b/src/topological_navigation/test/static_transformer.py similarity index 100% rename from topological_navigation/test/static_transformer.py rename to src/topological_navigation/test/static_transformer.py diff --git a/topological_navigation/test/test_convert_tmap.py b/src/topological_navigation/test/test_convert_tmap.py similarity index 100% rename from topological_navigation/test/test_convert_tmap.py rename to src/topological_navigation/test/test_convert_tmap.py diff --git a/topological_navigation/test/test_localisation2.py b/src/topological_navigation/test/test_localisation2.py similarity index 100% rename from topological_navigation/test/test_localisation2.py rename to src/topological_navigation/test/test_localisation2.py diff --git a/topological_navigation/test/test_navigation_graph.py b/src/topological_navigation/test/test_navigation_graph.py similarity index 100% rename from topological_navigation/test/test_navigation_graph.py rename to src/topological_navigation/test/test_navigation_graph.py diff --git a/topological_navigation/test/test_navigationcore.py b/src/topological_navigation/test/test_navigationcore.py similarity index 100% rename from topological_navigation/test/test_navigationcore.py rename to src/topological_navigation/test/test_navigationcore.py diff --git a/topological_navigation/test/test_networkx_utils.py b/src/topological_navigation/test/test_networkx_utils.py similarity index 100% rename from topological_navigation/test/test_networkx_utils.py rename to src/topological_navigation/test/test_networkx_utils.py diff --git a/topological_navigation/test/test_tmap_utils.py b/src/topological_navigation/test/test_tmap_utils.py similarity index 100% rename from topological_navigation/test/test_tmap_utils.py rename to src/topological_navigation/test/test_tmap_utils.py diff --git a/topological_navigation/test/test_validate_map.py b/src/topological_navigation/test/test_validate_map.py similarity index 100% rename from topological_navigation/test/test_validate_map.py rename to src/topological_navigation/test/test_validate_map.py diff --git a/topological_navigation/topological_navigation/__init__.py b/src/topological_navigation/topological_navigation/__init__.py similarity index 100% rename from topological_navigation/topological_navigation/__init__.py rename to src/topological_navigation/topological_navigation/__init__.py diff --git a/topological_navigation/topological_navigation/convert_tmap.py b/src/topological_navigation/topological_navigation/convert_tmap.py similarity index 100% rename from topological_navigation/topological_navigation/convert_tmap.py rename to src/topological_navigation/topological_navigation/convert_tmap.py diff --git a/topological_navigation/topological_navigation/navigation_graph.py b/src/topological_navigation/topological_navigation/navigation_graph.py similarity index 100% rename from topological_navigation/topological_navigation/navigation_graph.py rename to src/topological_navigation/topological_navigation/navigation_graph.py diff --git a/topological_navigation/topological_navigation/networkx_utils.py b/src/topological_navigation/topological_navigation/networkx_utils.py similarity index 100% rename from topological_navigation/topological_navigation/networkx_utils.py rename to src/topological_navigation/topological_navigation/networkx_utils.py diff --git a/topological_navigation/topological_navigation/scripts/__init__.py b/src/topological_navigation/topological_navigation/scripts/__init__.py similarity index 100% rename from topological_navigation/topological_navigation/scripts/__init__.py rename to src/topological_navigation/topological_navigation/scripts/__init__.py diff --git a/topological_navigation/topological_navigation/scripts/localisation2.py b/src/topological_navigation/topological_navigation/scripts/localisation2.py similarity index 100% rename from topological_navigation/topological_navigation/scripts/localisation2.py rename to src/topological_navigation/topological_navigation/scripts/localisation2.py diff --git a/topological_navigation/topological_navigation/scripts/manual_topomapping.py b/src/topological_navigation/topological_navigation/scripts/manual_topomapping.py similarity index 100% rename from topological_navigation/topological_navigation/scripts/manual_topomapping.py rename to src/topological_navigation/topological_navigation/scripts/manual_topomapping.py diff --git a/topological_navigation/topological_navigation/scripts/map_manager2.py b/src/topological_navigation/topological_navigation/scripts/map_manager2.py similarity index 100% rename from topological_navigation/topological_navigation/scripts/map_manager2.py rename to src/topological_navigation/topological_navigation/scripts/map_manager2.py diff --git a/topological_navigation/topological_navigation/scripts/navigation2.py b/src/topological_navigation/topological_navigation/scripts/navigation2.py similarity index 100% rename from topological_navigation/topological_navigation/scripts/navigation2.py rename to src/topological_navigation/topological_navigation/scripts/navigation2.py diff --git a/topological_navigation/topological_navigation/tmap_utils.py b/src/topological_navigation/topological_navigation/tmap_utils.py similarity index 100% rename from topological_navigation/topological_navigation/tmap_utils.py rename to src/topological_navigation/topological_navigation/tmap_utils.py diff --git a/topological_navigation/topological_navigation/validate_map.py b/src/topological_navigation/topological_navigation/validate_map.py similarity index 100% rename from topological_navigation/topological_navigation/validate_map.py rename to src/topological_navigation/topological_navigation/validate_map.py diff --git a/topological_navigation_msgs/CHANGELOG.rst b/src/topological_navigation_msgs/CHANGELOG.rst similarity index 100% rename from topological_navigation_msgs/CHANGELOG.rst rename to src/topological_navigation_msgs/CHANGELOG.rst diff --git a/topological_navigation_msgs/CMakeLists.txt b/src/topological_navigation_msgs/CMakeLists.txt similarity index 100% rename from topological_navigation_msgs/CMakeLists.txt rename to src/topological_navigation_msgs/CMakeLists.txt diff --git a/topological_navigation_msgs/LICENSE b/src/topological_navigation_msgs/LICENSE similarity index 100% rename from topological_navigation_msgs/LICENSE rename to src/topological_navigation_msgs/LICENSE diff --git a/topological_navigation_msgs/action/BuildTopPrediction.action b/src/topological_navigation_msgs/action/BuildTopPrediction.action similarity index 100% rename from topological_navigation_msgs/action/BuildTopPrediction.action rename to src/topological_navigation_msgs/action/BuildTopPrediction.action diff --git a/topological_navigation_msgs/action/ExecutePolicyMode.action b/src/topological_navigation_msgs/action/ExecutePolicyMode.action similarity index 100% rename from topological_navigation_msgs/action/ExecutePolicyMode.action rename to src/topological_navigation_msgs/action/ExecutePolicyMode.action diff --git a/topological_navigation_msgs/action/GotoNode.action b/src/topological_navigation_msgs/action/GotoNode.action similarity index 100% rename from topological_navigation_msgs/action/GotoNode.action rename to src/topological_navigation_msgs/action/GotoNode.action diff --git a/topological_navigation_msgs/msg/AddEdgeReq.msg b/src/topological_navigation_msgs/msg/AddEdgeReq.msg similarity index 100% rename from topological_navigation_msgs/msg/AddEdgeReq.msg rename to src/topological_navigation_msgs/msg/AddEdgeReq.msg diff --git a/topological_navigation_msgs/msg/AddNodeReq.msg b/src/topological_navigation_msgs/msg/AddNodeReq.msg similarity index 100% rename from topological_navigation_msgs/msg/AddNodeReq.msg rename to src/topological_navigation_msgs/msg/AddNodeReq.msg diff --git a/topological_navigation_msgs/msg/ClosestEdges.msg b/src/topological_navigation_msgs/msg/ClosestEdges.msg similarity index 100% rename from topological_navigation_msgs/msg/ClosestEdges.msg rename to src/topological_navigation_msgs/msg/ClosestEdges.msg diff --git a/topological_navigation_msgs/msg/CurrentEdge.msg b/src/topological_navigation_msgs/msg/CurrentEdge.msg similarity index 100% rename from topological_navigation_msgs/msg/CurrentEdge.msg rename to src/topological_navigation_msgs/msg/CurrentEdge.msg diff --git a/topological_navigation_msgs/msg/Edge.msg b/src/topological_navigation_msgs/msg/Edge.msg similarity index 100% rename from topological_navigation_msgs/msg/Edge.msg rename to src/topological_navigation_msgs/msg/Edge.msg diff --git a/topological_navigation_msgs/msg/ExecutePolicyModeFeedback.msg b/src/topological_navigation_msgs/msg/ExecutePolicyModeFeedback.msg similarity index 100% rename from topological_navigation_msgs/msg/ExecutePolicyModeFeedback.msg rename to src/topological_navigation_msgs/msg/ExecutePolicyModeFeedback.msg diff --git a/topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg b/src/topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg similarity index 100% rename from topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg rename to src/topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg diff --git a/topological_navigation_msgs/msg/GotoNodeFeedback.msg b/src/topological_navigation_msgs/msg/GotoNodeFeedback.msg similarity index 100% rename from topological_navigation_msgs/msg/GotoNodeFeedback.msg rename to src/topological_navigation_msgs/msg/GotoNodeFeedback.msg diff --git a/topological_navigation_msgs/msg/NavRoute.msg b/src/topological_navigation_msgs/msg/NavRoute.msg similarity index 100% rename from topological_navigation_msgs/msg/NavRoute.msg rename to src/topological_navigation_msgs/msg/NavRoute.msg diff --git a/topological_navigation_msgs/msg/NavStatistics.msg b/src/topological_navigation_msgs/msg/NavStatistics.msg similarity index 100% rename from topological_navigation_msgs/msg/NavStatistics.msg rename to src/topological_navigation_msgs/msg/NavStatistics.msg diff --git a/topological_navigation_msgs/msg/SetInfluenceZoneReq.msg b/src/topological_navigation_msgs/msg/SetInfluenceZoneReq.msg similarity index 100% rename from topological_navigation_msgs/msg/SetInfluenceZoneReq.msg rename to src/topological_navigation_msgs/msg/SetInfluenceZoneReq.msg diff --git a/topological_navigation_msgs/msg/TopologicalMap.msg b/src/topological_navigation_msgs/msg/TopologicalMap.msg similarity index 100% rename from topological_navigation_msgs/msg/TopologicalMap.msg rename to src/topological_navigation_msgs/msg/TopologicalMap.msg diff --git a/topological_navigation_msgs/msg/TopologicalNode.msg b/src/topological_navigation_msgs/msg/TopologicalNode.msg similarity index 100% rename from topological_navigation_msgs/msg/TopologicalNode.msg rename to src/topological_navigation_msgs/msg/TopologicalNode.msg diff --git a/topological_navigation_msgs/msg/TopologicalOccupiedNode.msg b/src/topological_navigation_msgs/msg/TopologicalOccupiedNode.msg similarity index 100% rename from topological_navigation_msgs/msg/TopologicalOccupiedNode.msg rename to src/topological_navigation_msgs/msg/TopologicalOccupiedNode.msg diff --git a/topological_navigation_msgs/msg/TopologicalRoute.msg b/src/topological_navigation_msgs/msg/TopologicalRoute.msg similarity index 100% rename from topological_navigation_msgs/msg/TopologicalRoute.msg rename to src/topological_navigation_msgs/msg/TopologicalRoute.msg diff --git a/topological_navigation_msgs/msg/UpdateEdgeConfigReq.msg b/src/topological_navigation_msgs/msg/UpdateEdgeConfigReq.msg similarity index 100% rename from topological_navigation_msgs/msg/UpdateEdgeConfigReq.msg rename to src/topological_navigation_msgs/msg/UpdateEdgeConfigReq.msg diff --git a/topological_navigation_msgs/msg/Vertex.msg b/src/topological_navigation_msgs/msg/Vertex.msg similarity index 100% rename from topological_navigation_msgs/msg/Vertex.msg rename to src/topological_navigation_msgs/msg/Vertex.msg diff --git a/topological_navigation_msgs/package.xml b/src/topological_navigation_msgs/package.xml similarity index 100% rename from topological_navigation_msgs/package.xml rename to src/topological_navigation_msgs/package.xml diff --git a/topological_navigation_msgs/srv/EvaluateEdge.srv b/src/topological_navigation_msgs/srv/EvaluateEdge.srv similarity index 100% rename from topological_navigation_msgs/srv/EvaluateEdge.srv rename to src/topological_navigation_msgs/srv/EvaluateEdge.srv diff --git a/topological_navigation_msgs/srv/EvaluateNode.srv b/src/topological_navigation_msgs/srv/EvaluateNode.srv similarity index 100% rename from topological_navigation_msgs/srv/EvaluateNode.srv rename to src/topological_navigation_msgs/srv/EvaluateNode.srv diff --git a/topological_navigation_msgs/srv/GetRouteBetween.srv b/src/topological_navigation_msgs/srv/GetRouteBetween.srv similarity index 100% rename from topological_navigation_msgs/srv/GetRouteBetween.srv rename to src/topological_navigation_msgs/srv/GetRouteBetween.srv diff --git a/topological_navigation_msgs/srv/GetRouteTo.srv b/src/topological_navigation_msgs/srv/GetRouteTo.srv similarity index 100% rename from topological_navigation_msgs/srv/GetRouteTo.srv rename to src/topological_navigation_msgs/srv/GetRouteTo.srv diff --git a/topological_navigation_msgs/srv/LoadTopoNavTestScenario.srv b/src/topological_navigation_msgs/srv/LoadTopoNavTestScenario.srv similarity index 100% rename from topological_navigation_msgs/srv/LoadTopoNavTestScenario.srv rename to src/topological_navigation_msgs/srv/LoadTopoNavTestScenario.srv diff --git a/topological_navigation_msgs/srv/LocalisePose.srv b/src/topological_navigation_msgs/srv/LocalisePose.srv similarity index 100% rename from topological_navigation_msgs/srv/LocalisePose.srv rename to src/topological_navigation_msgs/srv/LocalisePose.srv diff --git a/topological_navigation_msgs/srv/ReconfAtEdges.srv b/src/topological_navigation_msgs/srv/ReconfAtEdges.srv similarity index 100% rename from topological_navigation_msgs/srv/ReconfAtEdges.srv rename to src/topological_navigation_msgs/srv/ReconfAtEdges.srv diff --git a/topological_navigation_msgs/srv/RunTopoNavTestScenario.srv b/src/topological_navigation_msgs/srv/RunTopoNavTestScenario.srv similarity index 100% rename from topological_navigation_msgs/srv/RunTopoNavTestScenario.srv rename to src/topological_navigation_msgs/srv/RunTopoNavTestScenario.srv diff --git a/topological_navigation_msgs/srv/WriteTopologicalMap.srv b/src/topological_navigation_msgs/srv/WriteTopologicalMap.srv similarity index 100% rename from topological_navigation_msgs/srv/WriteTopologicalMap.srv rename to src/topological_navigation_msgs/srv/WriteTopologicalMap.srv diff --git a/topological_navigation_visual/launch/topological_map_visualiser.launch.py b/src/topological_navigation_visual/launch/topological_map_visualiser.launch.py similarity index 100% rename from topological_navigation_visual/launch/topological_map_visualiser.launch.py rename to src/topological_navigation_visual/launch/topological_map_visualiser.launch.py diff --git a/topological_navigation_visual/package.xml b/src/topological_navigation_visual/package.xml similarity index 100% rename from topological_navigation_visual/package.xml rename to src/topological_navigation_visual/package.xml diff --git a/topological_navigation_visual/resource/topological_navigation_visual b/src/topological_navigation_visual/resource/topological_navigation_visual similarity index 100% rename from topological_navigation_visual/resource/topological_navigation_visual rename to src/topological_navigation_visual/resource/topological_navigation_visual diff --git a/topological_navigation_visual/setup.cfg b/src/topological_navigation_visual/setup.cfg similarity index 100% rename from topological_navigation_visual/setup.cfg rename to src/topological_navigation_visual/setup.cfg diff --git a/topological_navigation_visual/setup.py b/src/topological_navigation_visual/setup.py similarity index 100% rename from topological_navigation_visual/setup.py rename to src/topological_navigation_visual/setup.py diff --git a/topological_navigation_visual/test/__init__.py b/src/topological_navigation_visual/test/__init__.py similarity index 100% rename from topological_navigation_visual/test/__init__.py rename to src/topological_navigation_visual/test/__init__.py diff --git a/topological_navigation_visual/topological_navigation_visual/__init__.py b/src/topological_navigation_visual/topological_navigation_visual/__init__.py similarity index 100% rename from topological_navigation_visual/topological_navigation_visual/__init__.py rename to src/topological_navigation_visual/topological_navigation_visual/__init__.py diff --git a/topological_navigation_visual/topological_navigation_visual/policy_marker.py b/src/topological_navigation_visual/topological_navigation_visual/policy_marker.py similarity index 100% rename from topological_navigation_visual/topological_navigation_visual/policy_marker.py rename to src/topological_navigation_visual/topological_navigation_visual/policy_marker.py diff --git a/topological_navigation_visual/topological_navigation_visual/scripts/__init__.py b/src/topological_navigation_visual/topological_navigation_visual/scripts/__init__.py similarity index 100% rename from topological_navigation_visual/topological_navigation_visual/scripts/__init__.py rename to src/topological_navigation_visual/topological_navigation_visual/scripts/__init__.py diff --git a/topological_navigation_visual/topological_navigation_visual/scripts/topological_map_visualiser.py b/src/topological_navigation_visual/topological_navigation_visual/scripts/topological_map_visualiser.py similarity index 100% rename from topological_navigation_visual/topological_navigation_visual/scripts/topological_map_visualiser.py rename to src/topological_navigation_visual/topological_navigation_visual/scripts/topological_map_visualiser.py diff --git a/topological_navigation_visual/topological_navigation_visual/scripts/topological_visual.py b/src/topological_navigation_visual/topological_navigation_visual/scripts/topological_visual.py similarity index 100% rename from topological_navigation_visual/topological_navigation_visual/scripts/topological_visual.py rename to src/topological_navigation_visual/topological_navigation_visual/scripts/topological_visual.py diff --git a/topological_navigation/launch/topological_navigation.launch.py b/topological_navigation/launch/topological_navigation.launch.py deleted file mode 100644 index 7a147cbb..00000000 --- a/topological_navigation/launch/topological_navigation.launch.py +++ /dev/null @@ -1,166 +0,0 @@ -#!/usr/bin/env python3 -# Copyright (c) 2026, topological_navigation contributors -# Licensed under the MIT License. -"""Launch the full topological navigation stack with simulation. - -Launches (in order): - 1. Map Manager -- loads and publishes the topological map - 2. Localisation -- determines current/closest node - 3. Fake Nav2 simulator -- virtual robot + fake action servers - 4. Topological map visualiser -- RViz markers - 5. RViz -- with a pre-configured display layout - -Usage ------ -Default (mixed_actions_map):: - - ros2 launch topological_navigation topological_navigation.launch.py - -Custom map file:: - - ros2 launch topological_navigation topological_navigation.launch.py \ - map_path:=/absolute/path/to/my_map.tmap2.yaml -""" - -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, TimerAction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - topo_nav_share = get_package_share_directory('topological_navigation') - - # --- Default paths --- - default_map = os.path.join( - topo_nav_share, 'config', 'mixed_actions_map.yaml', - ) - default_rviz = os.path.join( - topo_nav_share, 'rviz', 'topological_navigation.rviz', - ) - - return LaunchDescription([ - # ============================================================= - # Launch arguments - # ============================================================= - DeclareLaunchArgument( - 'map_path', default_value=default_map, - description='Absolute path to a .tmap2.yaml topological map', - ), - DeclareLaunchArgument( - 'rviz_config', default_value=default_rviz, - description='Path to the RViz config file', - ), - DeclareLaunchArgument( - 'initial_x', default_value='0.0', - description='Initial robot X position', - ), - DeclareLaunchArgument( - 'initial_y', default_value='0.0', - description='Initial robot Y position', - ), - DeclareLaunchArgument( - 'initial_yaw', default_value='0.0', - description='Initial robot yaw (radians)', - ), - - # ============================================================= - # 1. Map Manager - # ============================================================= - Node( - package='topological_navigation', - executable='map_manager2.py', - name='topological_map_manager_2', - output='screen', - arguments=[LaunchConfiguration('map_path')], - ), - - # ============================================================= - # 2. Localisation (delayed 2 s so map is published first) - # ============================================================= - TimerAction( - period=2.0, - actions=[ - Node( - package='topological_navigation', - executable='localisation2.py', - name='topological_localisation', - output='screen', - ), - ], - ), - - # ============================================================= - # 3. Fake Nav2 simulator (delayed 2 s) - # ============================================================= - TimerAction( - period=2.0, - actions=[ - Node( - package='topological_nav_simulator', - executable='fake_nav2_server', - name='fake_nav2_server', - output='screen', - parameters=[{ - 'initial_x': LaunchConfiguration('initial_x'), - 'initial_y': LaunchConfiguration('initial_y'), - 'initial_yaw': LaunchConfiguration('initial_yaw'), - }], - ), - ], - ), - - # ============================================================= - # 4. Topological navigation server (delayed 3 s) - # ============================================================= - TimerAction( - period=3.0, - actions=[ - Node( - package='topological_navigation', - executable='navigation2.py', - name='topological_navigation', - output='screen', - ), - ], - ), - - # ============================================================= - # 5. Topological map visualiser (delayed 4 s) - # ============================================================= - TimerAction( - period=4.0, - actions=[ - Node( - package='topological_navigation_visual', - executable='topological_map_visualiser.py', - name='topological_map_visualiser', - output='screen', - parameters=[{ - 'edit_mode': True, - }], - ), - ], - ), - - # ============================================================= - # 6. RViz (delayed 5 s) - # ============================================================= - TimerAction( - period=5.0, - actions=[ - Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', - arguments=[ - '-d', LaunchConfiguration('rviz_config'), - ], - ), - ], - ), - ])