├── .dockerignore ├── .flake8 ├── .github └── workflows │ ├── docker_image.yml │ └── python-package.yml ├── .gitignore ├── .pep8 ├── Dockerfile ├── LICENSE ├── README.md ├── docs └── demo.gif ├── mcp.json ├── package.xml ├── pyproject.toml ├── requirements.txt ├── src └── nav2_mcp_server │ ├── __init__.py │ ├── __main__.py │ ├── config.py │ ├── exceptions.py │ ├── navigation.py │ ├── py.typed │ ├── resources.py │ ├── server.py │ ├── tools.py │ ├── transforms.py │ └── utils.py ├── tests ├── __init__.py ├── conftest.py ├── test_config.py ├── test_exceptions.py ├── test_navigation.py ├── test_resources.py ├── test_server.py ├── test_tools.py ├── test_transforms.py └── test_utils.py └── uv.lock /.dockerignore: -------------------------------------------------------------------------------- 1 | # Git 2 | .git 3 | .gitignore 4 | 5 | # Python cache 6 | __pycache__/ 7 | *.py[cod] 8 | *$py.class 9 | *.so 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | 27 | # Virtual environments 28 | .env 29 | .venv 30 | env/ 31 | venv/ 32 | ENV/ 33 | env.bak/ 34 | venv.bak/ 35 | 36 | # IDE 37 | .vscode/ 38 | .idea/ 39 | *.swp 40 | *.swo 41 | *~ 42 | 43 | # OS 44 | .DS_Store 45 | .DS_Store? 46 | ._* 47 | .Spotlight-V100 48 | .Trashes 49 | ehthumbs.db 50 | Thumbs.db 51 | 52 | # Documentation 53 | *.md 54 | docs/ 55 | 56 | # Tests 57 | tests/ 58 | .pytest_cache/ 59 | .coverage 60 | .tox/ 61 | 62 | # Development files 63 | Dockerfile* 64 | docker-compose*.yml 65 | .dockerignore -------------------------------------------------------------------------------- /.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | max-line-length = 100 3 | inline-quotes = single 4 | exclude = 5 | .git, 6 | __pycache__, 7 | build, 8 | dist, 9 | .eggs, 10 | *.egg-info, 11 | .tox, 12 | .venv, 13 | venv 14 | ignore = 15 | E203, 16 | W503 -------------------------------------------------------------------------------- /.github/workflows/docker_image.yml: -------------------------------------------------------------------------------- 1 | name: Docker 2 | 3 | on: 4 | workflow_dispatch: 5 | workflow_run: 6 | workflows: ["Build"] 7 | types: 8 | - completed 9 | 10 | permissions: 11 | contents: read 12 | packages: write 13 | 14 | env: 15 | IMAGE: ghcr.io/${{ github.repository_owner }}/nav2_mcp_server 16 | 17 | concurrency: 18 | group: ${{ github.workflow }}-${{ github.ref }} 19 | cancel-in-progress: true 20 | 21 | jobs: 22 | build-and-push: 23 | if: ${{ github.event_name == 'workflow_dispatch' || github.event.workflow_run.conclusion == 'success' }} 24 | runs-on: ubuntu-latest 25 | steps: 26 | - name: Checkout 27 | uses: actions/checkout@v4.2.2 28 | with: 29 | ref: ${{ github.event.workflow_run.head_branch }} 30 | 31 | - name: Set up Docker Buildx 32 | uses: docker/setup-buildx-action@v3.3.0 33 | id: nav2_mcp_server-builder 34 | 35 | - name: Cache Docker layers 36 | uses: actions/cache@v4.2.0 37 | with: 38 | path: /tmp/.buildx-cache 39 | key: ${{ runner.os }}-buildx-${{ github.sha }} 40 | restore-keys: | 41 | ${{ runner.os }}-buildx- 42 | 43 | - name: Login to GitHub Container Registry 44 | uses: docker/login-action@v3.1.0 45 | with: 46 | registry: ghcr.io 47 | username: ${{ github.actor }} 48 | password: ${{ secrets.GHCR_TOKEN }} 49 | 50 | - name: Build and push Docker image 51 | uses: docker/build-push-action@v6 52 | with: 53 | builder: ${{ steps.nav2_mcp_server-builder.outputs.name }} 54 | context: . 55 | push: true 56 | platforms: linux/amd64 57 | tags: | 58 | ${{ env.IMAGE }}:latest 59 | ${{ env.IMAGE }}:${{ github.sha }} 60 | cache-from: type=local,src=/tmp/.buildx-cache 61 | cache-to: type=local,dest=/tmp/.buildx-cache-new 62 | 63 | 64 | # Temp fix 65 | # https://github.com/docker/build-push-action/issues/252 66 | # https://github.com/moby/buildkit/issues/1896 67 | - name: Move cache 68 | run: | 69 | rm -rf /tmp/.buildx-cache 70 | mv /tmp/.buildx-cache-new /tmp/.buildx-cache -------------------------------------------------------------------------------- /.github/workflows/python-package.yml: -------------------------------------------------------------------------------- 1 | # This workflow will install Python dependencies, run tests and lint with a variety of Python versions 2 | # For more information see: https://help.github.com/actions/language-and-framework-guides/using-python-with-github-actions 3 | 4 | name: Build 5 | permissions: 6 | contents: read 7 | pull-requests: write 8 | 9 | on: 10 | push: 11 | pull_request: 12 | 13 | jobs: 14 | linter: 15 | runs-on: ubuntu-latest 16 | strategy: 17 | max-parallel: 4 18 | matrix: 19 | python-version: [3.12] 20 | steps: 21 | - uses: actions/checkout@v4.2.2 22 | - name: Install uv 23 | uses: astral-sh/setup-uv@v5.3.0 24 | with: 25 | python-version: ${{ matrix.python-version }} 26 | enable-cache: true 27 | - name: Install the project 28 | run: uv sync --all-extras --dev 29 | - name: Check import order with isort 30 | run: uv run isort --check-only --diff src/ tests/ 31 | - name: Check format with autopep8 32 | run: uv run autopep8 --diff --exit-code --recursive src/ tests/ 33 | - name: Lint with flake8 34 | run: uv run flake8 src/ tests/ 35 | 36 | type: 37 | runs-on: ubuntu-latest 38 | container: ros:jazzy-ros-base-noble 39 | strategy: 40 | max-parallel: 4 41 | matrix: 42 | python-version: [3.12] 43 | steps: 44 | - uses: actions/checkout@v4.2.2 45 | with: {path: src/nav2_mcp_server} 46 | - name: Install dependencies 47 | run: | 48 | apt update 49 | rosdep update --rosdistro jazzy 50 | rosdep install --rosdistro jazzy --from-paths src --ignore-src -r -y 51 | - name: Install uv 52 | uses: astral-sh/setup-uv@v5.3.0 53 | with: 54 | python-version: ${{ matrix.python-version }} 55 | enable-cache: true 56 | - name: Install the project 57 | working-directory: src/nav2_mcp_server 58 | run: uv sync --all-extras --dev 59 | - name: Type check with mypy 60 | shell: bash 61 | working-directory: src/nav2_mcp_server 62 | run: | 63 | source /opt/ros/jazzy/setup.bash 64 | uv run mypy src/ tests/ 65 | 66 | test: 67 | runs-on: ubuntu-latest 68 | container: ros:jazzy-ros-base-noble 69 | strategy: 70 | max-parallel: 4 71 | matrix: 72 | python-version: [3.12] 73 | steps: 74 | - uses: actions/checkout@v4.2.2 75 | with: {path: src/nav2_mcp_server} 76 | - name: Install dependencies 77 | run: | 78 | apt update 79 | rosdep update --rosdistro jazzy 80 | rosdep install --rosdistro jazzy --from-paths src --ignore-src -r -y 81 | - name: Install uv 82 | uses: astral-sh/setup-uv@v5.3.0 83 | with: 84 | python-version: ${{ matrix.python-version }} 85 | enable-cache: true 86 | - name: Install the project 87 | working-directory: src/nav2_mcp_server 88 | run: uv sync --all-extras --dev 89 | - name: Run tests with pytest 90 | shell: bash 91 | working-directory: src/nav2_mcp_server 92 | run: | 93 | source /opt/ros/jazzy/setup.bash 94 | uv run pytest tests/ -v --cov=nav2_mcp_server --cov-report=xml --cov-report=term 95 | - name: Upload coverage reports to Codecov 96 | uses: codecov/codecov-action@v5.3.1 97 | with: 98 | token: ${{ secrets.CODECOV_TOKEN }} 99 | files: ./coverage.xml 100 | fail_ci_if_error: false 101 | verbose: true 102 | 103 | build: 104 | runs-on: ubuntu-latest 105 | container: ros:jazzy-ros-base-noble 106 | needs: [linter, type, test] 107 | strategy: 108 | max-parallel: 4 109 | matrix: 110 | python-version: [3.12] 111 | 112 | steps: 113 | - uses: actions/checkout@v4.2.2 114 | - name: Install uv 115 | uses: astral-sh/setup-uv@v5.3.0 116 | with: 117 | python-version: ${{ matrix.python-version }} 118 | enable-cache: true 119 | ignore-nothing-to-cache: true 120 | - name: Test with python ${{ matrix.python-version }} 121 | run: uv run --frozen tox -e py -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[codz] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py.cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # UV 98 | # Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | #uv.lock 102 | 103 | # poetry 104 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 105 | # This is especially recommended for binary packages to ensure reproducibility, and is more 106 | # commonly ignored for libraries. 107 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 108 | #poetry.lock 109 | #poetry.toml 110 | 111 | # pdm 112 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 113 | # pdm recommends including project-wide configuration in pdm.toml, but excluding .pdm-python. 114 | # https://pdm-project.org/en/latest/usage/project/#working-with-version-control 115 | #pdm.lock 116 | #pdm.toml 117 | .pdm-python 118 | .pdm-build/ 119 | 120 | # pixi 121 | # Similar to Pipfile.lock, it is generally recommended to include pixi.lock in version control. 122 | #pixi.lock 123 | # Pixi creates a virtual environment in the .pixi directory, just like venv module creates one 124 | # in the .venv directory. It is recommended not to include this directory in version control. 125 | .pixi 126 | 127 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 128 | __pypackages__/ 129 | 130 | # Celery stuff 131 | celerybeat-schedule 132 | celerybeat.pid 133 | 134 | # Redis 135 | *.rdb 136 | *.aof 137 | *.pid 138 | 139 | # RabbitMQ 140 | mnesia/ 141 | rabbitmq/ 142 | rabbitmq-data/ 143 | 144 | # ActiveMQ 145 | activemq-data/ 146 | 147 | # SageMath parsed files 148 | *.sage.py 149 | 150 | # Environments 151 | .env 152 | .envrc 153 | .venv 154 | env/ 155 | venv/ 156 | ENV/ 157 | env.bak/ 158 | venv.bak/ 159 | 160 | # Spyder project settings 161 | .spyderproject 162 | .spyproject 163 | 164 | # Rope project settings 165 | .ropeproject 166 | 167 | # mkdocs documentation 168 | /site 169 | 170 | # mypy 171 | .mypy_cache/ 172 | .dmypy.json 173 | dmypy.json 174 | 175 | # Pyre type checker 176 | .pyre/ 177 | 178 | # pytype static type analyzer 179 | .pytype/ 180 | 181 | # Cython debug symbols 182 | cython_debug/ 183 | 184 | # PyCharm 185 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 186 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 187 | # and can be added to the global gitignore or merged into this file. For a more nuclear 188 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 189 | #.idea/ 190 | 191 | # Abstra 192 | # Abstra is an AI-powered process automation framework. 193 | # Ignore directories containing user credentials, local state, and settings. 194 | # Learn more at https://abstra.io/docs 195 | .abstra/ 196 | 197 | # Visual Studio Code 198 | # Visual Studio Code specific template is maintained in a separate VisualStudioCode.gitignore 199 | # that can be found at https://github.com/github/gitignore/blob/main/Global/VisualStudioCode.gitignore 200 | # and can be added to the global gitignore or merged into this file. However, if you prefer, 201 | # you could uncomment the following to ignore the entire vscode folder 202 | # .vscode/ 203 | 204 | # Ruff stuff: 205 | .ruff_cache/ 206 | 207 | # PyPI configuration file 208 | .pypirc 209 | 210 | # Marimo 211 | marimo/_static/ 212 | marimo/_lsp/ 213 | __marimo__/ 214 | 215 | # Streamlit 216 | .streamlit/secrets.toml 217 | 218 | .vscode/* 219 | 220 | # Built Visual Studio Code Extensions 221 | *.vsix -------------------------------------------------------------------------------- /.pep8: -------------------------------------------------------------------------------- 1 | [pep8] 2 | max_line_length = 100 3 | # in-place = true 4 | # recursive = true 5 | aggressive = 3 6 | ignore = 7 | # E101: indentation contains mixed spaces and tabs 8 | # E11: Fix indentation 9 | # E121: continuation line under-indented for hanging indent 10 | # E122: continuation line missing indentation or outdented 11 | # E123: closing bracket does not match indentation of opening bracket's line 12 | # E124: closing bracket does not match visual indentation 13 | # E125: continuation line with same indent as next logical line 14 | # E126: continuation line over-indented for hanging indent 15 | # E127: continuation line over-indented for visual indent 16 | # E128: continuation line under-indented for visual indent 17 | # E129: visually indented line with same indent as next logical line 18 | # E131: continuation line unaligned for hanging indent 19 | # E201: whitespace after '(' 20 | # E202: whitespace before ')' 21 | # E203: whitespace before ':' 22 | # E211: whitespace before '(' 23 | # E221: multiple spaces before operator 24 | # E222: multiple spaces after operator 25 | # E223: tab before operator 26 | # E224: tab after operator 27 | # E225: missing whitespace around operator 28 | # E226: missing whitespace around arithmetic operator 29 | # E227: missing whitespace around bitwise or shift operator 30 | # E228: missing whitespace around modulo operator 31 | # E231: missing whitespace after ',' 32 | # E241: multiple spaces after ',' 33 | # E242: tab after ',' 34 | # E251: unexpected spaces around keyword / parameter equals 35 | # E252: missing whitespace around parameter equals 36 | # E26: Fix spacing after comment hash for inline comments 37 | # E265: block comment should start with '# ' 38 | # E266: too many leading '#' for block comment 39 | # E27: Fix extraneous whitespace around keywords 40 | # E301: expected 1 blank line, found 0 41 | # E302: expected 2 blank lines, found 0 42 | # E303: too many blank lines 43 | # E304: blank lines found after function decorator 44 | # E305: expected 2 blank lines after end of function or class 45 | # E306: expected 1 blank line before a nested definition 46 | # E401: multiple imports on one line 47 | # E402: module level import not at top of file 48 | # E501: line too long 49 | # E502: the backslash is redundant between brackets 50 | # E701: multiple statements on one line (colon) 51 | # E702: multiple statements on one line (semicolon) 52 | # E703: statement ends with a semicolon 53 | # E704: multiple statements on one line (def) 54 | # E711: comparison to None should be 'if cond is None:' 55 | # E712: comparison to True should be 'if cond is True:' or 'if cond:' 56 | # E713: test for membership should be 'not in' 57 | # E714: test for object identity should be 'is not' 58 | # E721: do not compare types, use 'isinstance()' 59 | # E722: do not use bare except, specify exception instead 60 | # E731: do not assign a lambda expression, use a def 61 | # W291: trailing whitespace 62 | # W292: no newline at end of file 63 | # W293: blank line contains whitespace 64 | # W391: blank line at end of file 65 | # W503: line break before binary operator 66 | # W504: line break after binary operator 67 | # W601: .has_key() is deprecated, use 'in' 68 | # W602: deprecated form of raising exception 69 | # W603: '<>' is deprecated, use '!=' 70 | # W604: backticks are deprecated, use 'repr()' 71 | select = E101,E11,E121,E122,E123,E124,E125,E126,E127,E128,E129,E131,E201,E202,E203,E211,E221,E222,E223,E224,E225,E226,E227,E228,E231,E241,E242,E251,E252,E26,E265,E266,E27,E301,E302,E303,E304,E305,E306,E401,E402,E501,E502,E701,E702,E703,E704,E711,E712,E713,E714,E721,E722,E731,W291,W292,W293,W391,W503,W504,W601,W602,W603,W604 72 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ghcr.io/alpine-ros/alpine-ros:jazzy-3.20-ros-core 2 | 3 | # ROS 2 environment variables 4 | ENV ROS_DISTRO=jazzy 5 | ENV RMW_IMPLEMENTATION=rmw_fastrtps_cpp 6 | ENV ROS_DOMAIN_ID=0 7 | ENV ROS_LOCALHOST_ONLY=1 8 | 9 | # Install necessary ROS2 packages 10 | RUN apk add --no-cache \ 11 | py3-pip \ 12 | ros-${ROS_DISTRO}-rmw-fastrtps-cpp \ 13 | ros-${ROS_DISTRO}-rosidl-generator-py \ 14 | ros-${ROS_DISTRO}-rosidl-typesupport-c \ 15 | ros-${ROS_DISTRO}-rosidl-typesupport-fastrtps-c \ 16 | ros-${ROS_DISTRO}-rosidl-typesupport-fastrtps-cpp \ 17 | ros-${ROS_DISTRO}-geometry-msgs \ 18 | ros-${ROS_DISTRO}-lifecycle-msgs \ 19 | ros-${ROS_DISTRO}-nav-msgs \ 20 | ros-${ROS_DISTRO}-nav2-msgs \ 21 | ros-${ROS_DISTRO}-tf2-ros-py \ 22 | ros-${ROS_DISTRO}-nav2-simple-commander \ 23 | && rm -rf /var/lib/apt/lists/* 24 | 25 | # Install uv using pip 26 | RUN pip3 install --no-cache-dir --break-system-packages uv 27 | 28 | # Set working directory 29 | WORKDIR /app 30 | 31 | # Copy dependency files first for better Docker layer caching 32 | COPY pyproject.toml . 33 | COPY uv.lock* . 34 | 35 | # Create a src folder 36 | RUN mkdir -p src 37 | 38 | # Install dependencies using uv 39 | RUN uv sync --frozen --no-dev 40 | 41 | # Copy source code 42 | COPY src/ src/ 43 | 44 | # Install the package in development mode 45 | RUN uv sync 46 | 47 | # MCP server startup command using uv 48 | ENTRYPOINT ["/ros_entrypoint.sh"] 49 | CMD ["uv", "run", "nav2_mcp_server"] -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [2025] [Alberto Tudela] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Nav2 MCP Server 2 | 3 | ![ROS2](https://img.shields.io/badge/ros2-jazzy-blue?logo=ros&logoColor=white) 4 | ![License](https://img.shields.io/github/license/ajtudela/nav2_mcp_server) 5 | [![Build](https://github.com/ajtudela/nav2_mcp_server/actions/workflows/python-package.yml/badge.svg?branch=main)](https://github.com/ajtudela/nav2_mcp_server/actions/workflows/python-package.yml) 6 | [![Docker image](https://github.com/ajtudela/nav2_mcp_server/actions/workflows/docker_image.yml/badge.svg?branch=main)](https://github.com/ajtudela/nav2_mcp_server/actions/workflows/docker_image.yml) 7 | ![Python](https://img.shields.io/python/required-version-toml?tomlFilePath=https%3A%2F%2Fraw.githubusercontent.com%2Fajtudela%2Fnav2_mcp_server%2Frefs%2Fheads%2Fmain%2Fpyproject.toml) 8 | ![Dynamic TOML Badge](https://img.shields.io/badge/dynamic/toml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fajtudela%2Fnav2_mcp_server%2Frefs%2Fheads%2Fmain%2Fpyproject.toml&query=%24.project.dependencies%5B2%5D&label=dependency) 9 | [![codecov](https://codecov.io/gh/ajtudela/nav2_mcp_server/graph/badge.svg?token=munojKDLxe)](https://codecov.io/gh/ajtudela/nav2_mcp_server) 10 | 11 | An MCP (Model Context Protocol) server that provides tools and resources to control and monitor Nav2 navigation operations, allowing seamless integration with Nav2-enabled robots through the MCP protocol. 12 | 13 | ![Demo of Nav2 MCP Server](docs/demo.gif) 14 | 15 | ## Tools 16 | 17 | | Tool | Description | Parameters | 18 | | ----------------------- | --------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | 19 | | **navigate_to_pose** | Navigate the robot to a specific pose (position and orientation) in the map frame | `x: float, y: float, yaw: float` | 20 | | **follow_waypoints** | Navigate the robot through a sequence of waypoints in order | `waypoints: str (JSON array)` | 21 | | **spin_robot** | Rotate the robot in place by a specified angle | `angle: float` | 22 | | **backup_robot** | Move the robot backward by a specified distance | `distance: float, speed: float` | 23 | | **dock_robot** | Dock the robot to a charging station or dock | `x: float, y: float, yaw: float, dock_id: str, dock_type: str, nav_to_dock: bool` | 24 | | **undock_robot** | Undock the robot from a charging station or dock | `dock_type: str` | 25 | | **clear_costmaps** | Clear robot navigation costmaps to remove stale obstacle data | `costmap_type: str` | 26 | | **get_robot_pose** | Get the current position and orientation of the robot | — | 27 | | **cancel_navigation** | Cancel the currently active navigation task | — | 28 | | **nav2_lifecycle** | Control Nav2 lifecycle (startup or shutdown) | `action: str` | 29 | | **get_path** | Compute a navigation path between two poses | `start_x: float, start_y: float, start_yaw: float, goal_x: float, goal_y: float, goal_yaw: float, planner_id: str` | 30 | | **get_path_from_robot** | Compute a navigation path from the robot's current pose to a goal pose | `goal_x: float, goal_y: float, goal_yaw: float, planner_id: str` | 31 | 32 | 33 | ## Environment Variables 34 | 35 | | Variable | Default | Description | 36 | | -------------------- | ------- | ------------------------------------------------------------ | 37 | | `ROS_DOMAIN_ID` | — | ROS 2 domain ID for network isolation (recommended to set) | 38 | | `ROS_LOCALHOST_ONLY` | — | Set to '1' to restrict ROS 2 communication to localhost only | 39 | 40 | ## Features 41 | 42 | * **Navigation control**: Navigate to specific poses, follow waypoint sequences, and execute precise robot movements 43 | * **Real-time status**: Monitor navigation progress, robot pose, and system status with comprehensive feedback 44 | * **Costmap management**: Clear stale obstacle data and manage navigation costmaps for optimal path planning 45 | * **Lifecycle management**: Control Nav2 system startup and shutdown for complete system control 46 | * **ROS 2 integration**: Full compatibility with Nav2 navigation stack and ROS 2 ecosystem 47 | * **Async operations**: Non-blocking navigation commands with progress monitoring and cancellation support 48 | 49 | ## Installation 50 | 51 | ### Dependencies 52 | 53 | - [Robot Operating System (ROS) 2](https://docs.ros.org/en/jazzy/): Middleware for robotics (Jazzy) 54 | - [fastmcp](https://github.com/jlowin/fastmcp): MCP server framework 55 | - [python](https://www.python.org/): Python programming language 56 | - [uv](https://github.com/astral-sh/uv): Python package manager (optional) 57 | 58 | 59 | ### Install with uv (recommended) 60 | 61 | Clone the repository and install with uv: 62 | 63 | ```bash 64 | git clone https://github.com/ajtudela/nav2_mcp_server.git 65 | cd nav2_mcp_server 66 | # Set up ROS 2 environment variables if needed 67 | export ROS_DOMAIN_ID=0 68 | uv sync 69 | ``` 70 | 71 | Or install directly from the repository: 72 | 73 | ```bash 74 | uv add git+https://github.com/ajtudela/nav2_mcp_server.git 75 | ``` 76 | 77 | ### Install with pip 78 | 79 | Install the package in development mode: 80 | 81 | ```bash 82 | git clone https://github.com/ajtudela/nav2_mcp_server.git 83 | cd nav2_mcp_server 84 | # Set up ROS 2 environment variables if needed 85 | export ROS_DOMAIN_ID=0 86 | python3 -m pip install . 87 | ``` 88 | 89 | Or install directly from the repository: 90 | 91 | ```bash 92 | python3 -m pip install git+https://github.com/ajtudela/nav2_mcp_server.git 93 | ``` 94 | 95 | ### Docker 96 | 97 | #### Build the image 98 | Build the image: 99 | 100 | ```bash 101 | docker build -t nav2_mcp_server:latest . 102 | ``` 103 | 104 | #### Pull the image 105 | Pull the latest image from the Docker registry: 106 | 107 | ```bash 108 | docker pull ghcr.io/ajtudela/nav2_mcp_server:latest 109 | ``` 110 | 111 | ## Usage 112 | 113 | ### Running with uv 114 | 115 | ```bash 116 | uv run nav2_mcp_server 117 | ``` 118 | 119 | ### Running with pip installation 120 | 121 | ```bash 122 | python3 -m nav2_mcp_server 123 | ``` 124 | 125 | ### Configuration example for Claude Desktop/Cursor/VSCode 126 | 127 | Add this configuration to your application's settings (mcp.json): 128 | 129 | #### Using uv (recommended) 130 | ```json 131 | { 132 | "nav2 mcp server": { 133 | "type": "stdio", 134 | "command": "uv", 135 | "args": [ 136 | "run", 137 | "--directory", 138 | "/path/to/nav2_mcp_server", 139 | "nav2_mcp_server" 140 | ], 141 | "env": { 142 | "ROS_DOMAIN_ID": "0", 143 | "ROS_LOCALHOST_ONLY": "1" 144 | } 145 | } 146 | } 147 | ``` 148 | 149 | #### Using pip installation 150 | ```json 151 | { 152 | "nav2 mcp server": { 153 | "type": "stdio", 154 | "command": "python3", 155 | "args": [ 156 | "-m", 157 | "nav2_mcp_server" 158 | ], 159 | "env": { 160 | "ROS_DOMAIN_ID": "0", 161 | "ROS_LOCALHOST_ONLY": "1" 162 | } 163 | } 164 | } 165 | ``` 166 | 167 | #### Using Docker 168 | ```json 169 | "nav2 mcp server": { 170 | "type": "stdio", 171 | "command": "docker", 172 | "args": [ 173 | "run", 174 | "-i", 175 | "--rm", 176 | "ghcr.io/ajtudela/nav2_mcp_server" 177 | ], 178 | "env": { 179 | "ROS_DOMAIN_ID": "0", 180 | "ROS_LOCALHOST_ONLY": "1" 181 | } 182 | } 183 | ``` -------------------------------------------------------------------------------- /docs/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ajtudela/nav2_mcp_server/8a501a9bfbd088043ba697dad1bcb4b6a07c63ce/docs/demo.gif -------------------------------------------------------------------------------- /mcp.json: -------------------------------------------------------------------------------- 1 | { 2 | "servers": { 3 | "nav2_mcp_server": { 4 | "type": "stdio", 5 | "command": "docker", 6 | "args": [ 7 | "run", 8 | "-i", 9 | "--rm", 10 | "-e", 11 | "ROS_DOMAIN_ID=YOUR_ROS_DOMAIN_ID", 12 | "nav2_mcp_server" 13 | ] 14 | } 15 | } 16 | } -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_mcp_server 5 | 1.0.0 6 | An MCP server that provides tools and resources to control and monitor robots using Nav2. 7 | Alberto Tudela 8 | Apache-2.0 9 | Alberto Tudela 10 | ament_python 11 | 12 | rclpy 13 | geometry_msgs 14 | nav2_simple_commander 15 | tf2_ros 16 | 17 | ament_copyright 18 | ament_flake8 19 | ament_pep257 20 | python3-pytest 21 | 22 | 23 | ament_python 24 | 25 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools>=61.0", "wheel"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | [project] 6 | name = "nav2-mcp-server" 7 | version = "1.0.0" 8 | authors = [ 9 | {name = "Alberto J. Tudela Roldán", email = "ajtudela@gmail.com"}, 10 | ] 11 | description = "MCP server wrapping Nav2 action clients" 12 | readme = "README.md" 13 | requires-python = ">=3.12,<3.13" 14 | dependencies = [ 15 | "anyio", 16 | "empy>=4.2", 17 | "fastmcp>=2.12.3", 18 | "lark>=1.3.0", 19 | "numpy>=2.3.0", 20 | ] 21 | 22 | [dependency-groups] 23 | dev = [ 24 | "autopep8>=2.0.0", 25 | "flake8>=7.0.0", 26 | "flake8-quotes>=3.3.0", 27 | "isort>=5.13.0", 28 | "tox>=4.24.1", 29 | "mypy>=1.14.1", 30 | "pytest>=8.4.2", 31 | "pytest-asyncio>=1.2.0", 32 | "pytest-cov>=6.0.0", 33 | ] 34 | 35 | [project.urls] 36 | "Homepage" = "https://github.com/ajtudela/nav2_mcp_server" 37 | "Bug Reports" = "https://github.com/ajtudela/nav2_mcp_server/issues" 38 | "Source" = "https://github.com/ajtudela/nav2_mcp_server" 39 | 40 | [project.scripts] 41 | nav2_mcp_server = "nav2_mcp_server.__main__:main" 42 | 43 | [tool.setuptools.packages.find] 44 | where = ["src"] 45 | 46 | [tool.setuptools.package-dir] 47 | "" = "src" 48 | 49 | [tool.isort] 50 | multi_line_output = 3 # Mode 3: Vertical Hanging Indent 51 | include_trailing_comma = true 52 | force_grid_wrap = 0 53 | use_parentheses = true 54 | ensure_newline_before_comments = true 55 | line_length = 100 56 | # Google Style section order 57 | force_single_line = false 58 | force_sort_within_sections = false 59 | lexicographical = false 60 | order_by_type = false 61 | group_by_package = false 62 | 63 | [tool.mypy] 64 | python_version = "3.12" 65 | warn_return_any = true 66 | warn_unused_configs = true 67 | disallow_untyped_defs = true 68 | disable_error_code = ["arg-type", "import-untyped"] 69 | 70 | [tool.pytest.ini_options] 71 | testpaths = ["tests"] 72 | python_files = ["test_*.py", "*_test.py"] 73 | python_classes = ["Test*"] 74 | python_functions = ["test_*"] 75 | addopts = "-v --tb=short --strict-markers" 76 | asyncio_mode = "auto" 77 | markers = [ 78 | "unit: marks tests as unit tests (deselect with '-m \"not unit\"')", 79 | "integration: marks tests as integration tests (deselect with '-m \"not integration\"')", 80 | "slow: marks tests as slow (deselect with '-m \"not slow\"')", 81 | ] 82 | 83 | [tool.coverage.run] 84 | omit = [ 85 | "src/nav2_mcp_server/__main__.py", 86 | "tests/*", 87 | ] 88 | 89 | [tool.coverage.report] 90 | exclude_lines = [ 91 | "pragma: no cover", 92 | "def __repr__", 93 | "if __name__ == .__main__.:", 94 | "raise AssertionError", 95 | "raise NotImplementedError", 96 | ] 97 | 98 | [tool.tox] 99 | env_list = ["3.12"] -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | "anyio", 2 | "empy>=4.2", 3 | "fastmcp>=2.12.3", 4 | "lark>=1.3.0", 5 | "numpy>=2.3.0", -------------------------------------------------------------------------------- /src/nav2_mcp_server/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Alberto J. Tudela Roldán 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Nav2 MCP Server package. 16 | 17 | This package provides a Model Context Protocol (MCP) server that exposes 18 | Nav2 navigation capabilities via FastMCP. 19 | """ 20 | 21 | __version__ = '1.0.0' 22 | __author__ = 'Alberto J. Tudela Roldán' 23 | __email__ = 'ajtudela@gmail.com' 24 | -------------------------------------------------------------------------------- /src/nav2_mcp_server/__main__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Alberto J. Tudela Roldán 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Main entry point for the nav2_mcp_server package.""" 16 | 17 | import asyncio 18 | 19 | from .server import main 20 | 21 | asyncio.run(main()) 22 | -------------------------------------------------------------------------------- /src/nav2_mcp_server/config.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Alberto J. Tudela Roldán 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Configuration settings for Nav2 MCP Server. 16 | 17 | This module provides centralized configuration management with default values 18 | and validation for the Nav2 MCP server application. 19 | """ 20 | 21 | import logging 22 | from dataclasses import dataclass 23 | from typing import Any, Dict, Optional 24 | 25 | 26 | @dataclass 27 | class NavigationConfig: 28 | """Configuration for navigation operations.""" 29 | 30 | default_backup_speed: float = 0.2 31 | default_tf_timeout: float = 0.5 32 | feedback_update_interval: int = 5 33 | 34 | # Frame names 35 | map_frame: str = 'map' 36 | base_link_frame: str = 'base_link' 37 | 38 | # Navigation limits 39 | max_waypoints: int = 100 40 | min_backup_distance: float = 0.01 41 | max_backup_distance: float = 10.0 42 | min_backup_speed: float = 0.01 43 | max_backup_speed: float = 1.0 44 | 45 | 46 | @dataclass 47 | class LoggingConfig: 48 | """Configuration for logging.""" 49 | 50 | level: int = logging.INFO 51 | log_format: str = '%(asctime)s - %(name)s - %(levelname)s - %(message)s' 52 | node_name: str = 'nav2_mcp_server' 53 | tf_node_name: str = 'nav2_mcp_server_tf_node' 54 | 55 | 56 | @dataclass 57 | class ServerConfig: 58 | """Main server configuration.""" 59 | 60 | transport: str = 'stdio' 61 | server_name: str = 'nav2-mcp-server' 62 | description: str = 'MCP server wrapping Nav2 action clients' 63 | 64 | # Resource URIs 65 | pose_uri: str = 'nav2://pose' 66 | 67 | 68 | class Config: 69 | """Main configuration class that combines all config sections.""" 70 | 71 | def __init__(self, config_dict: Optional[Dict[str, Any]] = None): 72 | """Initialize configuration with optional override dictionary. 73 | 74 | Parameters 75 | ---------- 76 | config_dict : dict, optional 77 | Dictionary to override default configuration values. 78 | """ 79 | self.navigation = NavigationConfig() 80 | self.logging = LoggingConfig() 81 | self.server = ServerConfig() 82 | 83 | if config_dict: 84 | self._apply_overrides(config_dict) 85 | 86 | self._validate() 87 | 88 | def _apply_overrides(self, config_dict: Dict[str, Any]) -> None: 89 | """Apply configuration overrides from dictionary. 90 | 91 | Parameters 92 | ---------- 93 | config_dict : dict 94 | Configuration overrides organized by section. 95 | """ 96 | if 'navigation' in config_dict: 97 | nav_config = config_dict['navigation'] 98 | for key, value in nav_config.items(): 99 | if hasattr(self.navigation, key): 100 | setattr(self.navigation, key, value) 101 | 102 | if 'logging' in config_dict: 103 | log_config = config_dict['logging'] 104 | for key, value in log_config.items(): 105 | if hasattr(self.logging, key): 106 | setattr(self.logging, key, value) 107 | 108 | if 'server' in config_dict: 109 | server_config = config_dict['server'] 110 | for key, value in server_config.items(): 111 | if hasattr(self.server, key): 112 | setattr(self.server, key, value) 113 | 114 | def _validate(self) -> None: 115 | """Validate configuration values. 116 | 117 | Raises 118 | ------ 119 | ValueError 120 | If any configuration value is invalid. 121 | """ 122 | nav = self.navigation 123 | 124 | if nav.default_backup_speed <= 0: 125 | raise ValueError('default_backup_speed must be positive') 126 | 127 | if nav.default_tf_timeout <= 0: 128 | raise ValueError('default_tf_timeout must be positive') 129 | 130 | if nav.max_waypoints <= 0: 131 | raise ValueError('max_waypoints must be positive') 132 | 133 | if nav.min_backup_distance >= nav.max_backup_distance: 134 | raise ValueError( 135 | 'min_backup_distance must be less than max_backup_distance') 136 | 137 | if nav.min_backup_speed >= nav.max_backup_speed: 138 | raise ValueError( 139 | 'min_backup_speed must be less than max_backup_speed') 140 | 141 | def to_dict(self) -> Dict[str, Any]: 142 | """Convert configuration to dictionary. 143 | 144 | Returns 145 | ------- 146 | dict 147 | Configuration as nested dictionary. 148 | """ 149 | return { 150 | 'navigation': self.navigation.__dict__, 151 | 'logging': self.logging.__dict__, 152 | 'server': self.server.__dict__ 153 | } 154 | 155 | 156 | # Global configuration instance 157 | _config: Optional[Config] = None 158 | 159 | 160 | def get_config() -> Config: 161 | """Get the global configuration instance. 162 | 163 | Returns 164 | ------- 165 | Config 166 | The global configuration instance. 167 | """ 168 | global _config 169 | if _config is None: 170 | _config = Config() 171 | return _config 172 | 173 | 174 | def set_config(config: Config) -> None: 175 | """Set the global configuration instance. 176 | 177 | Parameters 178 | ---------- 179 | config : Config 180 | The configuration instance to set as global. 181 | """ 182 | global _config 183 | _config = config 184 | 185 | 186 | def load_config_from_dict(config_dict: Dict[str, Any]) -> Config: 187 | """Load configuration from dictionary and set as global. 188 | 189 | Parameters 190 | ---------- 191 | config_dict : dict 192 | Configuration dictionary with override values. 193 | 194 | Returns 195 | ------- 196 | Config 197 | The loaded configuration instance. 198 | """ 199 | config = Config(config_dict) 200 | set_config(config) 201 | return config 202 | -------------------------------------------------------------------------------- /src/nav2_mcp_server/exceptions.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Alberto J. Tudela Roldán 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Custom exceptions for Nav2 MCP Server. 16 | 17 | This module defines specific exception classes for different error conditions 18 | that can occur during navigation operations. 19 | """ 20 | 21 | from enum import Enum 22 | from typing import Any, Optional 23 | 24 | 25 | class NavigationErrorCode(Enum): 26 | """Enumeration of navigation error codes.""" 27 | 28 | UNKNOWN = 0 29 | NAVIGATION_FAILED = 1 30 | NAVIGATION_CANCELED = 2 31 | TRANSFORM_UNAVAILABLE = 3 32 | NAV2_NOT_ACTIVE = 4 33 | INVALID_WAYPOINTS = 5 34 | INVALID_PARAMETERS = 6 35 | TIMEOUT = 7 36 | ROS_ERROR = 8 37 | 38 | 39 | class Nav2MCPError(Exception): 40 | """Base exception class for Nav2 MCP Server errors. 41 | 42 | Attributes 43 | ---------- 44 | message : str 45 | Human-readable error message. 46 | error_code : NavigationErrorCode 47 | Specific error code for the exception. 48 | details : dict, optional 49 | Additional error details or context. 50 | """ 51 | 52 | def __init__( 53 | self, 54 | message: str, 55 | error_code: NavigationErrorCode = NavigationErrorCode.UNKNOWN, 56 | details: Optional[dict] = None 57 | ): 58 | """Initialize the exception. 59 | 60 | Parameters 61 | ---------- 62 | message : str 63 | Human-readable error message. 64 | error_code : NavigationErrorCode, optional 65 | Specific error code (default: UNKNOWN). 66 | details : dict, optional 67 | Additional error details or context. 68 | """ 69 | super().__init__(message) 70 | self.message = message 71 | self.error_code = error_code 72 | self.details = details or {} 73 | 74 | def to_dict(self) -> dict: 75 | """Convert exception to dictionary representation. 76 | 77 | Returns 78 | ------- 79 | dict 80 | Dictionary containing error information. 81 | """ 82 | return { 83 | 'error': self.__class__.__name__, 84 | 'message': self.message, 85 | 'error_code': self.error_code.name, 86 | 'details': self.details 87 | } 88 | 89 | 90 | class NavigationError(Nav2MCPError): 91 | """Exception raised when navigation operations fail.""" 92 | 93 | def __init__( 94 | self, 95 | message: str, 96 | error_code: NavigationErrorCode = ( 97 | NavigationErrorCode.NAVIGATION_FAILED 98 | ), 99 | details: Optional[dict] = None 100 | ): 101 | super().__init__(message, error_code, details) 102 | 103 | 104 | class TransformError(Nav2MCPError): 105 | """Exception raised when transform operations fail.""" 106 | 107 | def __init__( 108 | self, 109 | message: str, 110 | error_code: NavigationErrorCode = ( 111 | NavigationErrorCode.TRANSFORM_UNAVAILABLE 112 | ), 113 | details: Optional[dict] = None 114 | ): 115 | super().__init__(message, error_code, details) 116 | 117 | 118 | class ConfigurationError(Nav2MCPError): 119 | """Exception raised when configuration is invalid.""" 120 | 121 | def __init__( 122 | self, 123 | message: str, 124 | error_code: NavigationErrorCode = ( 125 | NavigationErrorCode.INVALID_PARAMETERS 126 | ), 127 | details: Optional[dict] = None 128 | ): 129 | super().__init__(message, error_code, details) 130 | 131 | 132 | class TimeoutError(Nav2MCPError): 133 | """Exception raised when operations timeout.""" 134 | 135 | def __init__( 136 | self, 137 | message: str, 138 | timeout_duration: Optional[float] = None, 139 | details: Optional[dict] = None 140 | ): 141 | error_details = details or {} 142 | if timeout_duration is not None: 143 | error_details['timeout_duration'] = timeout_duration 144 | 145 | super().__init__(message, NavigationErrorCode.TIMEOUT, error_details) 146 | 147 | 148 | class ROSError(Nav2MCPError): 149 | """Exception raised when ROS operations fail.""" 150 | 151 | def __init__( 152 | self, 153 | message: str, 154 | ros_exception: Optional[Exception] = None, 155 | details: Optional[dict] = None 156 | ): 157 | error_details = details or {} 158 | if ros_exception is not None: 159 | error_details['ros_exception_type'] = type(ros_exception).__name__ 160 | error_details['ros_exception_message'] = str(ros_exception) 161 | 162 | super().__init__(message, NavigationErrorCode.ROS_ERROR, error_details) 163 | 164 | 165 | def create_navigation_error_from_result( 166 | result: Any, operation: str 167 | ) -> NavigationError: 168 | """Create a NavigationError based on navigation result. 169 | 170 | Parameters 171 | ---------- 172 | result : Any 173 | The navigation result from Nav2. 174 | operation : str 175 | Description of the operation that failed. 176 | 177 | Returns 178 | ------- 179 | NavigationError 180 | Appropriate NavigationError for the result. 181 | """ 182 | from nav2_simple_commander.robot_navigator import TaskResult 183 | 184 | if result == TaskResult.FAILED: 185 | return NavigationError( 186 | f'{operation} failed', 187 | NavigationErrorCode.NAVIGATION_FAILED, 188 | {'task_result': 'FAILED'} 189 | ) 190 | elif result == TaskResult.CANCELED: 191 | return NavigationError( 192 | f'{operation} was canceled', 193 | NavigationErrorCode.NAVIGATION_CANCELED, 194 | {'task_result': 'CANCELED'} 195 | ) 196 | else: 197 | return NavigationError( 198 | f'{operation} completed with unexpected result: {result}', 199 | NavigationErrorCode.UNKNOWN, 200 | {'task_result': str(result)} 201 | ) 202 | -------------------------------------------------------------------------------- /src/nav2_mcp_server/py.typed: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ajtudela/nav2_mcp_server/8a501a9bfbd088043ba697dad1bcb4b6a07c63ce/src/nav2_mcp_server/py.typed -------------------------------------------------------------------------------- /src/nav2_mcp_server/resources.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Alberto J. Tudela Roldán 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """MCP resources for Nav2 navigation information. 16 | 17 | This module provides MCP resource endpoints for accessing 18 | robot pose information. 19 | """ 20 | 21 | from fastmcp import FastMCP 22 | 23 | from .config import get_config 24 | from .transforms import get_transform_manager 25 | from .utils import MCPContextManager, safe_json_dumps 26 | 27 | 28 | def create_mcp_resources(mcp: FastMCP) -> None: 29 | """Create and register all MCP resources with the FastMCP instance. 30 | 31 | Parameters 32 | ---------- 33 | mcp : FastMCP 34 | The FastMCP server instance to register resources with. 35 | """ 36 | config = get_config() 37 | 38 | @mcp.resource( 39 | uri=config.server.pose_uri, 40 | name='Robot Pose', 41 | description='Current robot pose in map frame', 42 | mime_type='application/json' 43 | ) 44 | async def get_robot_pose_resource() -> str: 45 | """Resource endpoint for robot pose.""" 46 | try: 47 | transform_manager = get_transform_manager() 48 | context_manager = MCPContextManager() 49 | pose_info = transform_manager.get_robot_pose(context_manager) 50 | return safe_json_dumps(pose_info) 51 | except Exception as e: 52 | error_info = { 53 | 'error': 'Failed to get robot pose', 54 | 'message': str(e), 55 | 'error_type': type(e).__name__ 56 | } 57 | return safe_json_dumps(error_info) 58 | -------------------------------------------------------------------------------- /src/nav2_mcp_server/server.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Alberto J. Tudela Roldán 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """MCP server wrapping Nav2 action clients. 16 | 17 | This module exposes tools and resources for navigation via FastMCP. 18 | Provides comprehensive navigation capabilities including pose navigation, 19 | waypoint following, path planning, costmap operations, and lifecycle 20 | management. 21 | """ 22 | 23 | from typing import Optional 24 | 25 | import rclpy 26 | from fastmcp import FastMCP 27 | 28 | from .config import get_config 29 | from .navigation import get_navigation_manager 30 | from .resources import create_mcp_resources 31 | from .tools import create_mcp_tools 32 | from .transforms import get_transform_manager 33 | from .utils import setup_logging 34 | 35 | 36 | def create_server() -> FastMCP: 37 | """Create and configure the MCP server instance. 38 | 39 | Returns 40 | ------- 41 | FastMCP 42 | Configured MCP server instance. 43 | """ 44 | config = get_config() 45 | 46 | # Create MCP application 47 | mcp = FastMCP(config.server.server_name) 48 | 49 | # Register tools and resources 50 | create_mcp_tools(mcp) 51 | create_mcp_resources(mcp) 52 | 53 | return mcp 54 | 55 | 56 | async def main() -> None: 57 | """Run the Nav2 MCP server. 58 | 59 | Initializes ROS2, sets up logging, and starts the MCP server 60 | with stdio transport for integration with MCP clients. 61 | 62 | Notes 63 | ----- 64 | - Uses stdio transport for local MCP integration 65 | - Configures structured logging for debugging 66 | - Initializes global manager instances 67 | - Handles graceful shutdown of ROS2 nodes 68 | """ 69 | # Setup logging 70 | logger = setup_logging() 71 | logger.info('Starting Nav2 MCP Server...') 72 | 73 | # Initialize ROS2 74 | rclpy.init() 75 | logger.info('ROS2 initialized') 76 | 77 | # Initialize global managers to check connectivity 78 | nav_manager: Optional[object] = None 79 | transform_manager: Optional[object] = None 80 | 81 | try: 82 | # Pre-initialize managers to check ROS2 connectivity 83 | nav_manager = get_navigation_manager() 84 | transform_manager = get_transform_manager() 85 | logger.info('Navigation and transform managers initialized') 86 | 87 | # Create and start MCP server 88 | server = create_server() 89 | logger.info('Starting MCP server on stdio transport') 90 | await server.run_async(transport='stdio') 91 | 92 | except KeyboardInterrupt: 93 | logger.info('Server interrupted by user') 94 | except Exception as e: 95 | logger.error(f'Server error: {e}') 96 | raise 97 | finally: 98 | # Clean up ROS2 99 | logger.info('Shutting down ROS2...') 100 | if nav_manager and hasattr(nav_manager, 'destroy'): 101 | nav_manager.destroy() 102 | if transform_manager and hasattr(transform_manager, 'destroy'): 103 | transform_manager.destroy() 104 | rclpy.shutdown() 105 | logger.info('Nav2 MCP Server shutdown complete') 106 | -------------------------------------------------------------------------------- /src/nav2_mcp_server/tools.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Alberto J. Tudela Roldán 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """MCP tools for Nav2 navigation operations. 16 | 17 | This module provides the MCP tool definitions that wrap the navigation 18 | and transform operations with proper async/sync handling. 19 | """ 20 | 21 | from typing import Annotated, Optional 22 | 23 | import anyio 24 | from fastmcp import Context, FastMCP 25 | 26 | from .config import get_config 27 | from .navigation import get_navigation_manager 28 | from .transforms import get_transform_manager 29 | from .utils import ( # with_nav2_active_check, 30 | MCPContextManager, 31 | safe_json_dumps, 32 | with_context_logging, 33 | ) 34 | 35 | 36 | def create_mcp_tools(mcp: FastMCP) -> None: 37 | """Create and register all MCP tools with the FastMCP instance. 38 | 39 | Parameters 40 | ---------- 41 | mcp : FastMCP 42 | The FastMCP server instance to register tools with. 43 | """ 44 | config = get_config() 45 | 46 | # ------------------------------- 47 | # Navigation Tools 48 | # ------------------------------- 49 | 50 | @mcp.tool( 51 | name='navigate_to_pose', 52 | description="""Navigate the robot to a specific pose (position and 53 | orientation) in the map frame. 54 | 55 | Example usage: 56 | - navigate to position (2.0, 3.0) with orientation 1.57 57 | - go to x=2 y=3 with yaw=90 degrees 58 | - move to coordinates (2, 3) facing north 59 | """, 60 | tags={'navigate', 'go to', 'move to', 'navigate to pose', 'position'}, 61 | annotations={ 62 | 'title': 'Navigate To Pose', 63 | 'readOnlyHint': False, 64 | 'openWorldHint': False 65 | }, 66 | ) 67 | async def navigate_to_pose( 68 | x: Annotated[float, 'X coordinate of target pose in map frame'], 69 | y: Annotated[float, 'Y coordinate of target pose in map frame'], 70 | yaw: Annotated[ 71 | float, 72 | 'Orientation in radians (0=east, π/2=north, π=west, 3π/2=south)' 73 | ] = 0.0, 74 | ctx: Annotated[ 75 | Optional[Context], 76 | 'MCP context used for logging and progress msgs' 77 | ] = None, 78 | ) -> str: 79 | """Navigate robot to a specific pose with position and orientation.""" 80 | return await anyio.to_thread.run_sync( 81 | _navigate_to_pose_sync, x, y, yaw, ctx 82 | ) 83 | 84 | @mcp.tool( 85 | name='follow_waypoints', 86 | description="""Navigate the robot through a sequence of waypoints 87 | in order. 88 | 89 | Example usage: 90 | - follow waypoints at [[0, 0], [2, 0], [2, 2], [0, 2]] 91 | - navigate through points (1,1), (3,1), (3,3) 92 | - patrol between coordinates [[-1, -1], [1, 1], [1, -1]] 93 | """, 94 | tags={'follow', 'waypoints', 'sequence', 'multiple poses', 'patrol'}, 95 | annotations={ 96 | 'title': 'Follow Waypoints', 97 | 'readOnlyHint': False, 98 | 'openWorldHint': False 99 | }, 100 | ) 101 | async def follow_waypoints( 102 | waypoints: Annotated[ 103 | str, 104 | 'JSON string with waypoint coordinates [[x1,y1], [x2,y2], ...]' 105 | ], 106 | ctx: Annotated[ 107 | Optional[Context], 108 | 'MCP context for logging and progress' 109 | ] = None, 110 | ) -> str: 111 | """Follow a sequence of waypoints in the specified order.""" 112 | return await anyio.to_thread.run_sync( 113 | _follow_waypoints_sync, waypoints, ctx 114 | ) 115 | 116 | @mcp.tool( 117 | name='spin_robot', 118 | description="""Rotate the robot in place by a specified angle. 119 | 120 | Example usage: 121 | - spin robot 90 degrees clockwise 122 | - rotate robot by π/2 radians 123 | - turn robot around (π radians) 124 | """, 125 | tags={'spin', 'rotate', 'turn', 'in place', 'angle'}, 126 | annotations={ 127 | 'title': 'Spin Robot', 128 | 'readOnlyHint': False, 129 | 'openWorldHint': False 130 | }, 131 | ) 132 | async def spin_robot( 133 | angle: Annotated[ 134 | float, 135 | 'Angle to spin in radians (positive=counterclockwise, ' 136 | 'negative=clockwise)' 137 | ], 138 | ctx: Annotated[Optional[Context], 'MCP context for logging'] = None, 139 | ) -> str: 140 | """Rotate the robot in place by the specified angle.""" 141 | return await anyio.to_thread.run_sync(_spin_robot_sync, angle, ctx) 142 | 143 | @mcp.tool( 144 | name='backup_robot', 145 | description="""Move the robot backward by a specified distance. 146 | 147 | Example usage: 148 | - back up robot 1 meter at 0.2 m/s 149 | - reverse robot 0.5 meters slowly 150 | - backup 2 meters at normal speed 151 | """, 152 | tags={'backup', 'reverse', 'back up', 'move backward'}, 153 | annotations={ 154 | 'title': 'Backup Robot', 155 | 'readOnlyHint': False, 156 | 'openWorldHint': False 157 | }, 158 | ) 159 | async def backup_robot( 160 | distance: Annotated[ 161 | float, 'Distance to back up in meters (positive value)' 162 | ], 163 | speed: Annotated[ 164 | float, 165 | 'Backup speed in m/s (typically 0.1-0.5)' 166 | ] = 0.0, 167 | ctx: Annotated[Optional[Context], 'MCP context for logging'] = None, 168 | ) -> str: 169 | """Move the robot backward in a straight line.""" 170 | if speed is None: 171 | speed = config.navigation.default_backup_speed 172 | return await anyio.to_thread.run_sync( 173 | _backup_robot_sync, distance, speed, ctx 174 | ) 175 | 176 | @mcp.tool( 177 | name='dock_robot', 178 | description="""Dock the robot to a charging station or dock. 179 | 180 | This tool allows docking the robot either by specifying exact 181 | coordinates or by using a predefined dock ID. The robot can 182 | optionally navigate to a staging pose before performing the 183 | docking maneuver. 184 | 185 | Example usage: 186 | - dock robot at coordinates (5.0, 2.0) with 0 degree orientation 187 | - dock robot at dock ID 'dock_01' 188 | - dock robot with specific dock type or empty to use the default 189 | """, 190 | tags={'dock', 'docking', 'charging', 'station', 'park'}, 191 | annotations={ 192 | 'title': 'Dock Robot', 193 | 'readOnlyHint': False, 194 | 'openWorldHint': False 195 | }, 196 | ) 197 | async def dock_robot( 198 | x: Annotated[float, 'X coordinate of dock in map frame'], 199 | y: Annotated[float, 'Y coordinate of dock in map frame'], 200 | yaw: Annotated[ 201 | float, 'Orientation at dock in radians (0 = facing east)' 202 | ] = 0.0, 203 | dock_id: Annotated[ 204 | str, 'ID of predefined dock (alternative to coordinates)' 205 | ] = '', 206 | dock_type: Annotated[ 207 | str, 'Type of dock or empty to use the default' 208 | ] = '', 209 | nav_to_dock: Annotated[ 210 | bool, 'Whether to navigate to staging pose before docking' 211 | ] = True, 212 | ctx: Annotated[Optional[Context], 'MCP context for logging'] = None, 213 | ) -> str: 214 | """Dock the robot to a charging station or dock.""" 215 | return await anyio.to_thread.run_sync( 216 | _dock_robot_sync, x, y, yaw, dock_id, dock_type, nav_to_dock, ctx 217 | ) 218 | 219 | @mcp.tool( 220 | name='undock_robot', 221 | description="""Undock the robot from a charging station or dock. 222 | 223 | This tool allows the robot to undock from its current docking position 224 | and move to a safe undocked state. 225 | 226 | Example usage: 227 | - undock robot from current dock 228 | - undock robot from specific dock type 229 | """, 230 | tags={'undock', 'undocking', 'leave dock', 'depart'}, 231 | annotations={ 232 | 'title': 'Undock Robot', 233 | 'readOnlyHint': False, 234 | 'openWorldHint': False 235 | }, 236 | ) 237 | async def undock_robot( 238 | dock_type: Annotated[ 239 | str, 'Type of dock to undock from or empty to use default' 240 | ] = '', 241 | ctx: Annotated[Optional[Context], 'MCP context for logging'] = None, 242 | ) -> str: 243 | """Undock the robot from a charging station or dock.""" 244 | return await anyio.to_thread.run_sync( 245 | _undock_robot_sync, dock_type, ctx 246 | ) 247 | 248 | @mcp.tool( 249 | name='get_path', 250 | description="""Compute a navigation path between two poses 251 | (start and goal) in the map frame, using the planner. 252 | 253 | Example usage: 254 | - get path from (x1, y1, yaw1) to (x2, y2, yaw2) 255 | - plan path between two positions 256 | - compute path for robot navigation 257 | """, 258 | tags={'path', 'plan', 'compute', 'navigation', 'route', 'planner'}, 259 | annotations={ 260 | 'title': 'Get Path', 261 | 'readOnlyHint': True, 262 | 'openWorldHint': False 263 | }, 264 | ) 265 | async def get_path( 266 | start_x: Annotated[float, 'X coordinate of start pose in map frame'], 267 | start_y: Annotated[float, 'Y coordinate of start pose in map frame'], 268 | goal_x: Annotated[float, 'X coordinate of goal pose in map frame'], 269 | goal_y: Annotated[float, 'Y coordinate of goal pose in map frame'], 270 | start_yaw: Annotated[ 271 | float, 'Orientation of start pose in radians'] = 0.0, 272 | goal_yaw: Annotated[ 273 | float, 'Orientation of goal pose in radians'] = 0.0, 274 | planner_id: Annotated[str, 'Planner ID to use (optional)'] = '', 275 | ctx: Annotated[Optional[Context], 'MCP context for logging'] = None, 276 | ) -> str: 277 | """Compute a navigation path between two poses.""" 278 | return await anyio.to_thread.run_sync( 279 | _get_path_sync, 280 | start_x, start_y, start_yaw, 281 | goal_x, goal_y, goal_yaw, 282 | planner_id, True, ctx 283 | ) 284 | 285 | @mcp.tool( 286 | name='get_path_from_robot', 287 | description="""Compute a navigation path between the robot's 288 | current pose and a goal pose in the map frame, using the planner. 289 | 290 | Example usage: 291 | - get path from robot to (x2, y2, yaw2) 292 | - plan path to (x2, y2, yaw2) 293 | - compute path for robot navigation 294 | """, 295 | tags={'path', 'plan', 'compute', 'navigation', 'route', 'planner'}, 296 | annotations={ 297 | 'title': 'Get Path', 298 | 'readOnlyHint': True, 299 | 'openWorldHint': False 300 | }, 301 | ) 302 | async def get_path_from_robot( 303 | goal_x: Annotated[float, 'X coordinate of goal pose in map frame'], 304 | goal_y: Annotated[float, 'Y coordinate of goal pose in map frame'], 305 | goal_yaw: Annotated[ 306 | float, 'Orientation of goal pose in radians'] = 0.0, 307 | planner_id: Annotated[str, 'Planner ID to use (optional)'] = '', 308 | ctx: Annotated[Optional[Context], 'MCP context for logging'] = None, 309 | ) -> str: 310 | """Compute a navigation path between two poses.""" 311 | return await anyio.to_thread.run_sync( 312 | _get_path_sync, 313 | goal_x, goal_y, goal_yaw, 314 | goal_x, goal_y, goal_yaw, 315 | planner_id, False, ctx 316 | ) 317 | 318 | # ------------------------------- 319 | # Costmap Management Tools 320 | # ------------------------------- 321 | 322 | @mcp.tool( 323 | name='clear_costmaps', 324 | description="""Clear robot navigation costmaps to remove stale 325 | obstacle data. 326 | 327 | Example usage: 328 | - clear all costmaps 329 | - clear global costmap only 330 | - reset local costmap 331 | """, 332 | tags={'clear', 'costmap', 'obstacles', 'reset', 'navigation'}, 333 | annotations={ 334 | 'title': 'Clear Costmaps', 335 | 'readOnlyHint': False, 336 | 'openWorldHint': False 337 | }, 338 | ) 339 | async def clear_costmaps( 340 | costmap_type: Annotated[ 341 | str, 342 | 'Type of costmap to clear: "global", "local", or "all"' 343 | ] = 'all', 344 | ctx: Annotated[Optional[Context], 'MCP context for logging'] = None, 345 | ) -> str: 346 | """Clear navigation costmaps to remove stale obstacle information.""" 347 | return await anyio.to_thread.run_sync( 348 | _clear_costmaps_sync, costmap_type, ctx 349 | ) 350 | 351 | # ------------------------------- 352 | # Information Tools 353 | # ------------------------------- 354 | 355 | @mcp.tool( 356 | name='get_robot_pose', 357 | description="""Get the current position and orientation of the robot. 358 | 359 | Example usage: 360 | - where is the robot now? 361 | - get current robot position 362 | - show robot pose 363 | """, 364 | tags={'pose', 'position', 'location', 'robot', 'current', 'where'}, 365 | annotations={ 366 | 'title': 'Get Robot Pose', 367 | 'readOnlyHint': True, 368 | 'openWorldHint': False 369 | }, 370 | ) 371 | async def get_robot_pose( 372 | ctx: Annotated[Optional[Context], 'MCP context for logging'] = None, 373 | ) -> str: 374 | """Get the current pose (position and orientation) of the robot.""" 375 | return await anyio.to_thread.run_sync(_get_robot_pose_sync, ctx) 376 | 377 | @mcp.tool( 378 | name='cancel_navigation', 379 | description="""Cancel the currently active navigation task. 380 | 381 | Example usage: 382 | - cancel current navigation 383 | - stop the robot navigation 384 | - abort navigation task 385 | """, 386 | tags={'cancel', 'stop', 'abort', 'navigation', 'task'}, 387 | annotations={ 388 | 'title': 'Cancel Navigation', 389 | 'readOnlyHint': False, 390 | 'openWorldHint': False 391 | }, 392 | ) 393 | async def cancel_navigation( 394 | ctx: Annotated[Optional[Context], 'MCP context for logging'] = None, 395 | ) -> str: 396 | """Cancel any currently active navigation task.""" 397 | return await anyio.to_thread.run_sync(_cancel_navigation_sync, ctx) 398 | 399 | # ------------------------------- 400 | # Lifecycle Management Tools 401 | # ------------------------------- 402 | 403 | @mcp.tool( 404 | name='nav2_lifecycle', 405 | description="""Control Nav2 lifecycle (startup or shutdown). 406 | 407 | Example usage: 408 | - startup nav2 system 409 | - shutdown navigation 410 | - activate nav2 lifecycle 411 | """, 412 | tags={'lifecycle', 'startup', 'shutdown', 413 | 'nav2', 'activate', 'deactivate'}, 414 | annotations={ 415 | 'title': 'Nav2 Lifecycle Control', 416 | 'readOnlyHint': False, 417 | 'openWorldHint': False 418 | }, 419 | ) 420 | async def nav2_lifecycle( 421 | operation: Annotated[ 422 | str, 'Lifecycle operation: "startup" or "shutdown"' 423 | ], 424 | ctx: Annotated[Optional[Context], 'MCP context for logging'] = None, 425 | ) -> str: 426 | """Control the Nav2 navigation system lifecycle.""" 427 | return await anyio.to_thread.run_sync( 428 | _nav2_lifecycle_sync, operation, ctx 429 | ) 430 | 431 | 432 | # ------------------------------- 433 | # Synchronous Implementation Functions 434 | # ------------------------------- 435 | 436 | @with_context_logging 437 | # @with_nav2_active_check 438 | def _navigate_to_pose_sync( 439 | x: float, y: float, yaw: float = 0.0, 440 | ctx: Optional[Context] = None, 441 | context_manager: Optional[MCPContextManager] = None 442 | ) -> str: 443 | """Navigate robot to pose synchronously.""" 444 | nav_manager = get_navigation_manager() 445 | return nav_manager.navigate_to_pose(x, y, yaw, context_manager) 446 | 447 | 448 | @with_context_logging 449 | # @with_nav2_active_check 450 | def _follow_waypoints_sync( 451 | waypoints_str: str, 452 | ctx: Optional[Context] = None, 453 | context_manager: Optional[MCPContextManager] = None 454 | ) -> str: 455 | """Follow waypoints synchronously.""" 456 | nav_manager = get_navigation_manager() 457 | return nav_manager.follow_waypoints(waypoints_str, context_manager) 458 | 459 | 460 | @with_context_logging 461 | # @with_nav2_active_check 462 | def _spin_robot_sync( 463 | angle: float, 464 | ctx: Optional[Context] = None, 465 | context_manager: Optional[MCPContextManager] = None 466 | ) -> str: 467 | """Spin robot synchronously.""" 468 | nav_manager = get_navigation_manager() 469 | return nav_manager.spin_robot(angle, context_manager) 470 | 471 | 472 | @with_context_logging 473 | # @with_nav2_active_check 474 | def _backup_robot_sync( 475 | distance: float, speed: float, 476 | ctx: Optional[Context] = None, 477 | context_manager: Optional[MCPContextManager] = None 478 | ) -> str: 479 | """Back up robot synchronously.""" 480 | nav_manager = get_navigation_manager() 481 | return nav_manager.backup_robot(distance, speed, context_manager) 482 | 483 | 484 | @with_context_logging 485 | def _clear_costmaps_sync( 486 | costmap_type: str, 487 | ctx: Optional[Context] = None, 488 | context_manager: Optional[MCPContextManager] = None 489 | ) -> str: 490 | """Clear costmaps synchronously.""" 491 | nav_manager = get_navigation_manager() 492 | return nav_manager.clear_costmaps(costmap_type, context_manager) 493 | 494 | 495 | @with_context_logging 496 | def _get_robot_pose_sync( 497 | ctx: Optional[Context] = None, 498 | context_manager: Optional[MCPContextManager] = None 499 | ) -> str: 500 | """Get robot pose synchronously.""" 501 | transform_manager = get_transform_manager() 502 | pose_info = transform_manager.get_robot_pose(context_manager) 503 | return safe_json_dumps(pose_info) 504 | 505 | 506 | @with_context_logging 507 | def _cancel_navigation_sync( 508 | ctx: Optional[Context] = None, 509 | context_manager: Optional[MCPContextManager] = None 510 | ) -> str: 511 | """Cancel navigation synchronously.""" 512 | nav_manager = get_navigation_manager() 513 | return nav_manager.cancel_navigation(context_manager) 514 | 515 | 516 | @with_context_logging 517 | def _nav2_lifecycle_sync( 518 | operation: str, 519 | ctx: Optional[Context] = None, 520 | context_manager: Optional[MCPContextManager] = None 521 | ) -> str: 522 | """Perform Nav2 lifecycle operation synchronously.""" 523 | nav_manager = get_navigation_manager() 524 | 525 | if operation == 'startup': 526 | return nav_manager.lifecycle_startup(context_manager) 527 | elif operation == 'shutdown': 528 | return nav_manager.lifecycle_shutdown(context_manager) 529 | else: 530 | raise ValueError( 531 | f'Invalid operation "{operation}". Use: "startup" or "shutdown"') 532 | 533 | 534 | @with_context_logging 535 | def _dock_robot_sync( 536 | x: Optional[float], 537 | y: Optional[float], 538 | yaw: float, 539 | dock_id: str, 540 | dock_type: str, 541 | nav_to_dock: bool, 542 | ctx: Optional[Context] = None, 543 | context_manager: Optional[MCPContextManager] = None 544 | ) -> str: 545 | """Dock robot synchronously.""" 546 | nav_manager = get_navigation_manager() 547 | 548 | # Create dock pose if coordinates are provided 549 | dock_pose = None 550 | if x is not None and y is not None: 551 | dock_pose = nav_manager.create_pose_stamped(x, y, yaw) 552 | 553 | return nav_manager.dock_robot( 554 | dock_pose=dock_pose, 555 | dock_id=dock_id, 556 | dock_type=dock_type, 557 | nav_to_dock=nav_to_dock, 558 | context_manager=context_manager 559 | ) 560 | 561 | 562 | @with_context_logging 563 | def _undock_robot_sync( 564 | dock_type: str, 565 | ctx: Optional[Context] = None, 566 | context_manager: Optional[MCPContextManager] = None 567 | ) -> str: 568 | """Undock robot synchronously.""" 569 | nav_manager = get_navigation_manager() 570 | return nav_manager.undock_robot(dock_type, context_manager) 571 | 572 | 573 | @with_context_logging 574 | def _get_path_sync( 575 | start_x: float, 576 | start_y: float, 577 | start_yaw: float, 578 | goal_x: float, 579 | goal_y: float, 580 | goal_yaw: float, 581 | planner_id: str, 582 | use_start: bool, 583 | ctx: Optional[Context] = None, 584 | context_manager: Optional[MCPContextManager] = None 585 | ) -> str: 586 | """Get path synchronously.""" 587 | nav_manager = get_navigation_manager() 588 | return nav_manager.get_path( 589 | start_x, start_y, start_yaw, goal_x, goal_y, goal_yaw, planner_id, 590 | use_start, context_manager 591 | ) 592 | -------------------------------------------------------------------------------- /src/nav2_mcp_server/transforms.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Alberto J. Tudela Roldán 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Transform management for Nav2 MCP Server. 16 | 17 | This module provides classes and functions for managing TF transforms 18 | and robot pose operations. 19 | """ 20 | 21 | import math 22 | from typing import Any, Dict, Optional 23 | 24 | import rclpy 25 | from rclpy.node import Node 26 | from tf2_ros import Buffer, TransformException, TransformListener 27 | 28 | from .config import get_config 29 | from .exceptions import NavigationErrorCode, TransformError 30 | from .utils import MCPContextManager, safe_json_dumps 31 | 32 | 33 | class TransformManager: 34 | """Manages TF transforms and robot pose operations.""" 35 | 36 | def __init__(self) -> None: 37 | """Initialize the transform manager.""" 38 | self.config = get_config() 39 | self._node: Optional[Node] = None 40 | self._tf_buffer: Optional[Buffer] = None 41 | self._tf_listener: Optional[TransformListener] = None 42 | 43 | def _ensure_tf_setup(self) -> None: 44 | """Ensure TF components are initialized.""" 45 | if self._node is None: 46 | self._node = Node(self.config.logging.tf_node_name) 47 | self._tf_buffer = Buffer() 48 | self._tf_listener = TransformListener(self._tf_buffer, self._node) 49 | 50 | def get_robot_pose( 51 | self, context_manager: MCPContextManager 52 | ) -> Dict[str, Any]: 53 | """Get current robot pose in map frame. 54 | 55 | Parameters 56 | ---------- 57 | context_manager : MCPContextManager 58 | Context manager for logging. 59 | 60 | Returns 61 | ------- 62 | dict 63 | Robot pose information including position and orientation. 64 | 65 | Raises 66 | ------ 67 | TransformError 68 | If transform lookup fails. 69 | """ 70 | context_manager.info_sync('Getting current robot pose...') 71 | 72 | self._ensure_tf_setup() 73 | 74 | # Ensure tf_buffer is available after setup 75 | if self._tf_buffer is None: 76 | raise TransformError( 77 | 'TF buffer not initialized', 78 | NavigationErrorCode.TRANSFORM_UNAVAILABLE, 79 | {} 80 | ) 81 | 82 | try: 83 | # Wait for transform to become available 84 | transform = None 85 | while transform is None: 86 | rclpy.spin_once( 87 | self._node, 88 | timeout_sec=self.config.navigation.default_tf_timeout 89 | ) 90 | 91 | if self._tf_buffer.can_transform( 92 | self.config.navigation.map_frame, 93 | self.config.navigation.base_link_frame, 94 | rclpy.time.Time() 95 | ): 96 | transform = self._tf_buffer.lookup_transform( 97 | self.config.navigation.map_frame, # target frame 98 | self.config.navigation.base_link_frame, # source frame 99 | rclpy.time.Time(), # get latest available transform 100 | ) 101 | break 102 | 103 | if transform is None: 104 | raise TransformError( 105 | 'Could not get transform after waiting', 106 | NavigationErrorCode.TRANSFORM_UNAVAILABLE, 107 | { 108 | 'target_frame': self.config.navigation.map_frame, 109 | 'source_frame': self.config.navigation.base_link_frame 110 | } 111 | ) 112 | 113 | # Extract pose information 114 | pose_info = self._extract_pose_from_transform(transform) 115 | 116 | context_manager.info_sync( 117 | f'Current robot pose: {safe_json_dumps(pose_info)}' 118 | ) 119 | 120 | return pose_info 121 | 122 | except TransformException as ex: 123 | raise TransformError( 124 | f'Could not get transform from ' 125 | f'{self.config.navigation.base_link_frame} ' 126 | f'to {self.config.navigation.map_frame}: {str(ex)}', 127 | NavigationErrorCode.TRANSFORM_UNAVAILABLE, 128 | { 129 | 'target_frame': self.config.navigation.map_frame, 130 | 'source_frame': self.config.navigation.base_link_frame, 131 | 'tf_exception_type': type(ex).__name__, 132 | 'tf_exception_message': str(ex) 133 | } 134 | ) 135 | except Exception as e: 136 | raise TransformError( 137 | f'Unexpected error getting robot pose: {str(e)}', 138 | NavigationErrorCode.TRANSFORM_UNAVAILABLE, 139 | { 140 | 'error_type': type(e).__name__, 141 | 'error_message': str(e) 142 | } 143 | ) 144 | 145 | def _extract_pose_from_transform(self, transform: Any) -> Dict[str, Any]: 146 | """Extract pose information from transform. 147 | 148 | Parameters 149 | ---------- 150 | transform 151 | TF transform message. 152 | 153 | Returns 154 | ------- 155 | dict 156 | Pose information with position and orientation. 157 | """ 158 | # Extract position 159 | position = { 160 | 'x': transform.transform.translation.x, 161 | 'y': transform.transform.translation.y, 162 | 'z': transform.transform.translation.z 163 | } 164 | 165 | # Extract quaternion 166 | quat = { 167 | 'w': transform.transform.rotation.w, 168 | 'x': transform.transform.rotation.x, 169 | 'y': transform.transform.rotation.y, 170 | 'z': transform.transform.rotation.z 171 | } 172 | 173 | # Calculate yaw from quaternion (rotation around z-axis) 174 | yaw = self._quaternion_to_yaw( 175 | quat['w'], quat['x'], quat['y'], quat['z']) 176 | 177 | return { 178 | 'position': position, 179 | 'orientation': { 180 | 'quaternion': quat, 181 | 'yaw': yaw, 182 | 'yaw_degrees': math.degrees(yaw) 183 | }, 184 | 'frame_id': self.config.navigation.map_frame, 185 | 'child_frame_id': self.config.navigation.base_link_frame, 186 | 'timestamp': { 187 | 'sec': transform.header.stamp.sec, 188 | 'nanosec': transform.header.stamp.nanosec 189 | } 190 | } 191 | 192 | def _quaternion_to_yaw( 193 | self, w: float, x: float, y: float, z: float 194 | ) -> float: 195 | """Convert quaternion to yaw angle. 196 | 197 | Parameters 198 | ---------- 199 | w : float 200 | Quaternion w component. 201 | x : float 202 | Quaternion x component. 203 | y : float 204 | Quaternion y component. 205 | z : float 206 | Quaternion z component. 207 | 208 | Returns 209 | ------- 210 | float 211 | Yaw angle in radians. 212 | """ 213 | return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) 214 | 215 | def yaw_to_quaternion(self, yaw: float) -> Dict[str, float]: 216 | """Convert yaw angle to quaternion. 217 | 218 | Parameters 219 | ---------- 220 | yaw : float 221 | Yaw angle in radians. 222 | 223 | Returns 224 | ------- 225 | dict 226 | Quaternion components (w, x, y, z). 227 | """ 228 | return { 229 | 'w': math.cos(yaw / 2.0), 230 | 'x': 0.0, 231 | 'y': 0.0, 232 | 'z': math.sin(yaw / 2.0) 233 | } 234 | 235 | def destroy(self) -> None: 236 | """Clean up transform manager resources.""" 237 | if self._tf_listener: 238 | # TF listener is cleaned up automatically with the node 239 | self._tf_listener = None 240 | 241 | if self._tf_buffer: 242 | self._tf_buffer = None 243 | 244 | if self._node: 245 | self._node.destroy_node() 246 | self._node = None 247 | 248 | 249 | # Global transform manager instance 250 | _transform_manager: Optional[TransformManager] = None 251 | 252 | 253 | def get_transform_manager() -> TransformManager: 254 | """Get or create the global transform manager instance. 255 | 256 | Returns 257 | ------- 258 | TransformManager 259 | The global transform manager instance. 260 | """ 261 | global _transform_manager 262 | if _transform_manager is None: 263 | _transform_manager = TransformManager() 264 | return _transform_manager 265 | -------------------------------------------------------------------------------- /src/nav2_mcp_server/utils.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 Alberto J. Tudela Roldán 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Utilities for MCP context management and logging. 16 | 17 | This module provides helpers for managing MCP context, logging, 18 | and common async/sync operations. 19 | """ 20 | 21 | import functools 22 | import json 23 | import logging 24 | from typing import Any, Callable, Dict, Optional, TypeVar 25 | 26 | import anyio 27 | from fastmcp import Context 28 | 29 | from .config import get_config 30 | from .exceptions import Nav2MCPError, ROSError 31 | 32 | F = TypeVar('F', bound=Callable[..., Any]) 33 | 34 | 35 | class MCPContextManager: 36 | """Manager for MCP context operations and logging.""" 37 | 38 | def __init__(self, ctx: Optional[Context] = None): 39 | """Initialize context manager. 40 | 41 | Parameters 42 | ---------- 43 | ctx : Context, optional 44 | MCP context for logging and progress updates. 45 | """ 46 | self.ctx = ctx 47 | self.logger = logging.getLogger(__name__) 48 | 49 | async def info(self, message: str) -> None: 50 | """Log info message to both MCP context and logger. 51 | 52 | Parameters 53 | ---------- 54 | message : str 55 | Message to log. 56 | """ 57 | self.logger.info(message) 58 | if self.ctx: 59 | await self.ctx.info(message) 60 | 61 | async def error(self, message: str) -> None: 62 | """Log error message to both MCP context and logger. 63 | 64 | Parameters 65 | ---------- 66 | message : str 67 | Error message to log. 68 | """ 69 | self.logger.error(message) 70 | if self.ctx: 71 | await self.ctx.error(message) 72 | 73 | async def warning(self, message: str) -> None: 74 | """Log warning message to both MCP context and logger. 75 | 76 | Parameters 77 | ---------- 78 | message : str 79 | Warning message to log. 80 | """ 81 | self.logger.warning(message) 82 | if self.ctx: 83 | await self.ctx.warning(message) 84 | 85 | def info_sync(self, message: str) -> None: 86 | """Log info message from sync context. 87 | 88 | Parameters 89 | ---------- 90 | message : str 91 | Message to log. 92 | """ 93 | self.logger.info(message) 94 | if self.ctx: 95 | anyio.from_thread.run(self.ctx.info, message) 96 | 97 | def error_sync(self, message: str) -> None: 98 | """Log error message from sync context. 99 | 100 | Parameters 101 | ---------- 102 | message : str 103 | Error message to log. 104 | """ 105 | self.logger.error(message) 106 | if self.ctx: 107 | anyio.from_thread.run(self.ctx.error, message) 108 | 109 | def warning_sync(self, message: str) -> None: 110 | """Log warning message from sync context. 111 | 112 | Parameters 113 | ---------- 114 | message : str 115 | Warning message to log. 116 | """ 117 | self.logger.warning(message) 118 | if self.ctx: 119 | anyio.from_thread.run(self.ctx.warning, message) 120 | 121 | 122 | def setup_logging() -> logging.Logger: 123 | """Configure logging for the MCP server. 124 | 125 | Returns 126 | ------- 127 | logging.Logger 128 | Configured logger instance. 129 | """ 130 | config = get_config() 131 | logging.basicConfig( 132 | level=config.logging.level, 133 | format=config.logging.log_format 134 | ) 135 | return logging.getLogger(config.logging.node_name) 136 | 137 | 138 | def with_context_logging(func: F) -> F: 139 | """Decorator to add context logging to sync functions. 140 | 141 | Parameters 142 | ---------- 143 | func : callable 144 | Function to decorate. 145 | 146 | Returns 147 | ------- 148 | callable 149 | Decorated function with context logging. 150 | """ 151 | @functools.wraps(func) 152 | def wrapper(*args: Any, **kwargs: Any) -> Any: 153 | # Extract context from kwargs if present 154 | ctx = kwargs.get('ctx') 155 | context_manager = MCPContextManager(ctx) 156 | 157 | # Add context manager to kwargs 158 | kwargs['context_manager'] = context_manager 159 | 160 | try: 161 | return func(*args, **kwargs) 162 | except Nav2MCPError as e: 163 | error_msg = f'Navigation error in {func.__name__}: {e.message}' 164 | context_manager.error_sync(error_msg) 165 | return json.dumps(e.to_dict(), indent=2) 166 | except Exception as e: 167 | error_msg = f'Unexpected error in {func.__name__}: {str(e)}' 168 | context_manager.error_sync(error_msg) 169 | ros_error = ROSError(error_msg, e) 170 | return json.dumps(ros_error.to_dict(), indent=2) 171 | 172 | return wrapper # type: ignore[return-value] 173 | 174 | 175 | def with_nav2_active_check(func: F) -> F: 176 | """Decorator to ensure Nav2 is active before executing function. 177 | 178 | Parameters 179 | ---------- 180 | func : callable 181 | Function to decorate. 182 | 183 | Returns 184 | ------- 185 | callable 186 | Decorated function with Nav2 active check. 187 | """ 188 | @functools.wraps(func) 189 | def wrapper(*args: Any, **kwargs: Any) -> Any: 190 | from .navigation import get_navigator 191 | 192 | navigator = get_navigator() 193 | context_manager = kwargs.get('context_manager') 194 | 195 | try: 196 | if context_manager: 197 | context_manager.info_sync( 198 | 'Waiting for Nav2 to become active...') 199 | 200 | navigator.waitUntilNav2Active() 201 | 202 | if context_manager: 203 | context_manager.info_sync('Nav2 is active') 204 | 205 | return func(*args, **kwargs) 206 | 207 | except Exception as e: 208 | if context_manager: 209 | context_manager.error_sync(f'Nav2 activation failed: {str(e)}') 210 | raise ROSError('Failed to activate Nav2', e) 211 | 212 | return wrapper # type: ignore[return-value] 213 | 214 | 215 | def create_success_message(operation: str, details: Dict[str, Any]) -> str: 216 | """Create a standardized success message. 217 | 218 | Parameters 219 | ---------- 220 | operation : str 221 | The operation that succeeded. 222 | details : dict 223 | Additional details about the operation. 224 | 225 | Returns 226 | ------- 227 | str 228 | Formatted success message. 229 | """ 230 | detail_str = ', '.join([f'{k}={v}' for k, v in details.items()]) 231 | return f'Successfully {operation}: {detail_str}' 232 | 233 | 234 | def create_failure_message( 235 | operation: str, reason: str, details: Optional[Dict[str, Any]] = None 236 | ) -> str: 237 | """Create a standardized failure message. 238 | 239 | Parameters 240 | ---------- 241 | operation : str 242 | The operation that failed. 243 | reason : str 244 | The reason for failure. 245 | details : dict, optional 246 | Additional details about the failure. 247 | 248 | Returns 249 | ------- 250 | str 251 | Formatted failure message. 252 | """ 253 | message = f'Failed to {operation}: {reason}' 254 | if details: 255 | detail_str = ', '.join([f'{k}={v}' for k, v in details.items()]) 256 | message += f' ({detail_str})' 257 | return message 258 | 259 | 260 | def validate_numeric_range( 261 | value: float, 262 | min_value: float, 263 | max_value: float, 264 | parameter_name: str 265 | ) -> None: 266 | """Validate that a numeric value is within specified range. 267 | 268 | Parameters 269 | ---------- 270 | value : float 271 | Value to validate. 272 | min_value : float 273 | Minimum allowed value. 274 | max_value : float 275 | Maximum allowed value. 276 | parameter_name : str 277 | Name of the parameter for error messages. 278 | 279 | Raises 280 | ------ 281 | ValueError 282 | If value is outside the specified range. 283 | """ 284 | if not (min_value <= value <= max_value): 285 | raise ValueError( 286 | f'{parameter_name} must be between {min_value} and {max_value}, ' 287 | f'got {value}' 288 | ) 289 | 290 | 291 | def safe_json_dumps(obj: Any, indent: int = 2) -> str: 292 | """Safely serialize object to JSON with error handling. 293 | 294 | Parameters 295 | ---------- 296 | obj : Any 297 | Object to serialize. 298 | indent : int, optional 299 | JSON indentation (default: 2). 300 | 301 | Returns 302 | ------- 303 | str 304 | JSON string or error message if serialization fails. 305 | """ 306 | try: 307 | return json.dumps(obj, indent=indent, default=str) 308 | except (TypeError, ValueError) as e: 309 | return json.dumps({ 310 | 'error': 'JSON serialization failed', 311 | 'message': str(e), 312 | 'object_type': type(obj).__name__ 313 | }, indent=indent) 314 | -------------------------------------------------------------------------------- /tests/__init__.py: -------------------------------------------------------------------------------- 1 | """Test suite for Nav2 MCP Server.""" 2 | -------------------------------------------------------------------------------- /tests/conftest.py: -------------------------------------------------------------------------------- 1 | """Shared pytest fixtures for Nav2 MCP Server tests. 2 | 3 | This module provides reusable fixtures for testing the Nav2 MCP server, 4 | including mock ROS2 interfaces, sample navigation data, and server 5 | configurations. 6 | """ 7 | 8 | import math 9 | from typing import Any, Dict, Generator, List 10 | from unittest.mock import Mock, patch 11 | 12 | import pytest 13 | from fastmcp import FastMCP 14 | from geometry_msgs.msg import PoseStamped 15 | from tf2_ros import Buffer 16 | 17 | from nav2_mcp_server.server import create_server 18 | from nav2_mcp_server.utils import MCPContextManager 19 | 20 | 21 | def create_mock_config() -> Mock: 22 | """Create a mock configuration object with all required attributes. 23 | 24 | Returns 25 | ------- 26 | Mock 27 | A mock configuration object with properly configured attributes. 28 | """ 29 | mock_config = Mock() 30 | 31 | # Server config - create mock object with string returns 32 | server_mock = Mock() 33 | server_mock.server_name = 'nav2-mcp-server' 34 | server_mock.version = '0.1.0' 35 | server_mock.pose_uri = 'nav2://robot_pose' 36 | mock_config.server = server_mock 37 | 38 | # Logging config 39 | logging_mock = Mock() 40 | logging_mock.node_name = 'nav2_mcp_server' 41 | logging_mock.tf_node_name = 'tf_listener' 42 | logging_mock.log_level = 'INFO' 43 | logging_mock.log_format = '%(asctime)s - %(levelname)s - %(message)s' 44 | mock_config.logging = logging_mock 45 | 46 | # Navigation config 47 | navigation_mock = Mock() 48 | navigation_mock.map_frame = 'map' 49 | navigation_mock.base_link_frame = 'base_link' 50 | navigation_mock.default_tf_timeout = 1.0 51 | navigation_mock.max_waypoints = 100 52 | mock_config.navigation = navigation_mock 53 | 54 | return mock_config 55 | 56 | 57 | @pytest.fixture 58 | def mock_ros2_initialized() -> Generator[None, None, None]: 59 | """Mock ROS2 initialization.""" 60 | with patch('rclpy.init'): 61 | with patch('rclpy.shutdown'): 62 | yield 63 | 64 | 65 | @pytest.fixture 66 | def mock_navigation_manager() -> Mock: 67 | """Provide a mock NavigationManager for testing. 68 | 69 | Returns 70 | ------- 71 | Mock 72 | Mock NavigationManager with preset return values and side effects. 73 | """ 74 | mock_nav_manager = Mock() 75 | 76 | # Mock successful navigation result 77 | mock_nav_manager.navigate_to_pose.return_value = ( 78 | 'Navigation to pose started successfully' 79 | ) 80 | mock_nav_manager.follow_waypoints.return_value = ( 81 | 'Waypoint following started successfully' 82 | ) 83 | mock_nav_manager.spin_robot.return_value = ( 84 | 'Spin operation started successfully' 85 | ) 86 | mock_nav_manager.backup_robot.return_value = ( 87 | 'Backup operation started successfully' 88 | ) 89 | mock_nav_manager.dock_robot.return_value = ( 90 | 'Docking operation started successfully' 91 | ) 92 | mock_nav_manager.undock_robot.return_value = ( 93 | 'Undocking operation started successfully' 94 | ) 95 | mock_nav_manager.clear_costmaps.return_value = ( 96 | 'Costmaps cleared successfully' 97 | ) 98 | mock_nav_manager.cancel_navigation.return_value = ( 99 | 'Navigation cancelled successfully' 100 | ) 101 | mock_nav_manager.lifecycle_startup.return_value = ( 102 | 'Nav2 lifecycle startup completed successfully' 103 | ) 104 | mock_nav_manager.lifecycle_shutdown.return_value = ( 105 | 'Nav2 lifecycle shutdown completed successfully' 106 | ) 107 | 108 | # Mock path planning results (returned as JSON strings) 109 | import json 110 | mock_nav_manager.get_path.return_value = json.dumps({ 111 | 'path': [ 112 | {'x': 0.0, 'y': 0.0, 'theta': 0.0}, 113 | {'x': 1.0, 'y': 1.0, 'theta': 0.785}, 114 | {'x': 2.0, 'y': 2.0, 'theta': 1.571} 115 | ], 116 | 'length': 2.828, 117 | 'status': 'success' 118 | }) 119 | 120 | return mock_nav_manager 121 | 122 | 123 | @pytest.fixture 124 | def mock_transform_manager() -> Mock: 125 | """Provide a mock TransformManager for testing. 126 | 127 | Returns 128 | ------- 129 | Mock 130 | Mock TransformManager with preset return values. 131 | """ 132 | mock_tf_manager = Mock() 133 | 134 | # Mock robot pose 135 | mock_tf_manager.get_robot_pose.return_value = { 136 | 'pose': { 137 | 'position': {'x': 1.0, 'y': 2.0, 'z': 0.0}, 138 | 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0} 139 | }, 140 | 'header': { 141 | 'frame_id': 'map', 142 | 'stamp': {'sec': 1640995200, 'nanosec': 0} 143 | }, 144 | 'yaw': 0.0, 145 | 'status': 'success' 146 | } 147 | 148 | return mock_tf_manager 149 | 150 | 151 | @pytest.fixture 152 | def mock_context_manager() -> Mock: 153 | """Provide a mock MCPContextManager for testing. 154 | 155 | Returns 156 | ------- 157 | Mock 158 | Mock MCPContextManager for context handling. 159 | """ 160 | mock_context = Mock(spec=MCPContextManager) 161 | mock_context.info.return_value = None 162 | mock_context.warning.return_value = None 163 | mock_context.error.return_value = None 164 | return mock_context 165 | 166 | 167 | @pytest.fixture 168 | def sample_pose() -> Dict[str, Any]: 169 | """Provide a sample pose for testing. 170 | 171 | Returns 172 | ------- 173 | Dict[str, Any] 174 | Sample pose dictionary. 175 | """ 176 | return { 177 | 'position': {'x': 1.0, 'y': 2.0, 'z': 0.0}, 178 | 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.707, 'w': 0.707} 179 | } 180 | 181 | 182 | @pytest.fixture 183 | def sample_waypoints() -> List[Dict[str, Any]]: 184 | """Provide sample waypoints for testing. 185 | 186 | Returns 187 | ------- 188 | List[Dict[str, Any]] 189 | List of waypoint dictionaries. 190 | """ 191 | return [ 192 | { 193 | 'position': {'x': 1.0, 'y': 1.0, 'z': 0.0}, 194 | 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0} 195 | }, 196 | { 197 | 'position': {'x': 2.0, 'y': 2.0, 'z': 0.0}, 198 | 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.707, 'w': 0.707} 199 | }, 200 | { 201 | 'position': {'x': 3.0, 'y': 1.0, 'z': 0.0}, 202 | 'orientation': {'x': 0.0, 'y': 0.0, 'z': 1.0, 'w': 0.0} 203 | } 204 | ] 205 | 206 | 207 | @pytest.fixture 208 | def sample_path() -> Dict[str, Any]: 209 | """Provide a sample path for testing. 210 | 211 | Returns 212 | ------- 213 | Dict[str, Any] 214 | Sample path dictionary with poses and metadata. 215 | """ 216 | return { 217 | 'path': [ 218 | {'x': 0.0, 'y': 0.0, 'theta': 0.0}, 219 | {'x': 0.5, 'y': 0.5, 'theta': 0.785}, 220 | {'x': 1.0, 'y': 1.0, 'theta': 1.571}, 221 | {'x': 1.5, 'y': 1.0, 'theta': 3.14}, 222 | {'x': 2.0, 'y': 1.0, 'theta': 0.0} 223 | ], 224 | 'length': 3.414, 225 | 'status': 'success', 226 | 'planning_time': 0.15 227 | } 228 | 229 | 230 | @pytest.fixture 231 | def mock_ros_pose_stamped() -> Mock: 232 | """Provide a mock ROS PoseStamped message. 233 | 234 | Returns 235 | ------- 236 | Mock 237 | Mock PoseStamped message with realistic structure. 238 | """ 239 | pose_stamped = Mock(spec=PoseStamped) 240 | pose_stamped.header.frame_id = 'map' 241 | pose_stamped.header.stamp.sec = 1640995200 242 | pose_stamped.header.stamp.nanosec = 0 243 | 244 | pose_stamped.pose.position.x = 1.0 245 | pose_stamped.pose.position.y = 2.0 246 | pose_stamped.pose.position.z = 0.0 247 | 248 | pose_stamped.pose.orientation.x = 0.0 249 | pose_stamped.pose.orientation.y = 0.0 250 | pose_stamped.pose.orientation.z = 0.0 251 | pose_stamped.pose.orientation.w = 1.0 252 | 253 | return pose_stamped 254 | 255 | 256 | @pytest.fixture 257 | def mock_tf_buffer() -> Mock: 258 | """Provide a mock TF2 Buffer for testing. 259 | 260 | Returns 261 | ------- 262 | Mock 263 | Mock TF2 Buffer with transform lookup capabilities. 264 | """ 265 | mock_buffer = Mock(spec=Buffer) 266 | 267 | # Mock transform lookup 268 | mock_transform = Mock() 269 | mock_transform.transform.translation.x = 1.0 270 | mock_transform.transform.translation.y = 2.0 271 | mock_transform.transform.translation.z = 0.0 272 | mock_transform.transform.rotation.x = 0.0 273 | mock_transform.transform.rotation.y = 0.0 274 | mock_transform.transform.rotation.z = 0.0 275 | mock_transform.transform.rotation.w = 1.0 276 | mock_transform.header.frame_id = 'map' 277 | mock_transform.child_frame_id = 'base_link' 278 | 279 | mock_buffer.lookup_transform.return_value = mock_transform 280 | 281 | return mock_buffer 282 | 283 | 284 | @pytest.fixture 285 | def test_server() -> Generator[FastMCP, None, None]: 286 | """Provide the FastMCP server instance for testing. 287 | 288 | Returns 289 | ------- 290 | FastMCP 291 | The configured MCP server instance. 292 | """ 293 | with patch('nav2_mcp_server.server.get_config') as mock_get_config: 294 | mock_get_config.return_value = create_mock_config() 295 | yield create_server() 296 | 297 | 298 | # Navigation result fixtures 299 | @pytest.fixture 300 | def navigation_success_result() -> Dict[str, Any]: 301 | """Success result for navigation operations.""" 302 | return { 303 | 'status': 'success', 304 | 'message': 'Navigation completed successfully', 305 | 'result_code': 4, # SUCCEEDED 306 | 'duration': 15.5 307 | } 308 | 309 | 310 | @pytest.fixture 311 | def navigation_failure_result() -> Dict[str, Any]: 312 | """Failure result for navigation operations.""" 313 | return { 314 | 'status': 'failed', 315 | 'message': 'Navigation failed: Goal unreachable', 316 | 'result_code': 5, # ABORTED 317 | 'duration': 8.2 318 | } 319 | 320 | 321 | @pytest.fixture 322 | def lifecycle_nodes() -> List[str]: 323 | """List of Nav2 lifecycle nodes.""" 324 | return [ 325 | 'controller_server', 326 | 'smoother_server', 327 | 'planner_server', 328 | 'behavior_server', 329 | 'bt_navigator', 330 | 'waypoint_follower', 331 | 'velocity_smoother' 332 | ] 333 | 334 | 335 | # Error scenarios fixtures 336 | @pytest.fixture 337 | def ros_connection_error() -> Exception: 338 | """ROS connection error for testing.""" 339 | return ConnectionError('Failed to connect to ROS2 nodes') 340 | 341 | 342 | @pytest.fixture 343 | def navigation_timeout_error() -> Exception: 344 | """Navigation timeout error for testing.""" 345 | return TimeoutError('Navigation action timed out') 346 | 347 | 348 | @pytest.fixture 349 | def transform_lookup_error() -> Exception: 350 | """Transform lookup error for testing.""" 351 | return Exception("Transform lookup failed: Frame 'odom' does not exist") 352 | 353 | 354 | # Math utilities for testing 355 | def create_quaternion_from_yaw(yaw: float) -> Dict[str, float]: 356 | """Create quaternion from yaw angle. 357 | 358 | Parameters 359 | ---------- 360 | yaw : float 361 | Yaw angle in radians. 362 | 363 | Returns 364 | ------- 365 | Dict[str, float] 366 | Quaternion representation. 367 | """ 368 | return { 369 | 'x': 0.0, 370 | 'y': 0.0, 371 | 'z': math.sin(yaw / 2.0), 372 | 'w': math.cos(yaw / 2.0) 373 | } 374 | -------------------------------------------------------------------------------- /tests/test_exceptions.py: -------------------------------------------------------------------------------- 1 | """Tests for exceptions module. 2 | 3 | This module tests custom exception classes, error codes, 4 | and error handling utilities. 5 | """ 6 | 7 | from unittest.mock import Mock 8 | 9 | import pytest 10 | 11 | from nav2_mcp_server.exceptions import ( 12 | ConfigurationError, 13 | create_navigation_error_from_result, 14 | Nav2MCPError, 15 | NavigationError, 16 | NavigationErrorCode, 17 | ROSError, 18 | TimeoutError, 19 | TransformError, 20 | ) 21 | 22 | 23 | class TestNavigationErrorCode: 24 | """Tests for NavigationErrorCode enumeration.""" 25 | 26 | def test_error_code_values_exist(self) -> None: 27 | """Test that all error codes are defined. 28 | 29 | Verifies that the enum contains all expected error codes. 30 | """ 31 | assert hasattr(NavigationErrorCode, 'UNKNOWN') 32 | assert hasattr(NavigationErrorCode, 'NAVIGATION_FAILED') 33 | assert hasattr(NavigationErrorCode, 'NAVIGATION_CANCELED') 34 | assert hasattr(NavigationErrorCode, 'TRANSFORM_UNAVAILABLE') 35 | assert hasattr(NavigationErrorCode, 'NAV2_NOT_ACTIVE') 36 | assert hasattr(NavigationErrorCode, 'INVALID_WAYPOINTS') 37 | assert hasattr(NavigationErrorCode, 'INVALID_PARAMETERS') 38 | assert hasattr(NavigationErrorCode, 'TIMEOUT') 39 | assert hasattr(NavigationErrorCode, 'ROS_ERROR') 40 | 41 | def test_error_code_values_are_unique(self) -> None: 42 | """Test that error code values are unique. 43 | 44 | Verifies that each error code has a distinct value. 45 | """ 46 | codes = [code.value for code in NavigationErrorCode] 47 | assert len(codes) == len(set(codes)) 48 | 49 | def test_error_code_names(self) -> None: 50 | """Test error code name access. 51 | 52 | Verifies that error code names can be accessed. 53 | """ 54 | assert NavigationErrorCode.UNKNOWN.name == 'UNKNOWN' 55 | assert NavigationErrorCode.NAVIGATION_FAILED.name == 'NAVIGATION_FAILED' 56 | assert NavigationErrorCode.TIMEOUT.name == 'TIMEOUT' 57 | 58 | 59 | class TestNav2MCPError: 60 | """Tests for base Nav2MCPError exception class.""" 61 | 62 | def test_basic_exception_creation(self) -> None: 63 | """Test basic exception creation. 64 | 65 | Verifies that base exception can be created with 66 | minimal parameters. 67 | """ 68 | error = Nav2MCPError('Test error') 69 | 70 | assert str(error) == 'Test error' 71 | assert error.message == 'Test error' 72 | assert error.error_code == NavigationErrorCode.UNKNOWN 73 | assert error.details == {} 74 | 75 | def test_exception_with_error_code(self) -> None: 76 | """Test exception creation with error code. 77 | 78 | Verifies that specific error codes can be set. 79 | """ 80 | error = Nav2MCPError( 81 | 'Navigation failed', 82 | error_code=NavigationErrorCode.NAVIGATION_FAILED 83 | ) 84 | 85 | assert error.message == 'Navigation failed' 86 | assert error.error_code == NavigationErrorCode.NAVIGATION_FAILED 87 | 88 | def test_exception_with_details(self) -> None: 89 | """Test exception creation with details. 90 | 91 | Verifies that additional details can be attached. 92 | """ 93 | details = {'waypoint': 5, 'reason': 'obstacle'} 94 | error = Nav2MCPError('Error with details', details=details) 95 | 96 | assert error.details == details 97 | assert error.details['waypoint'] == 5 98 | assert error.details['reason'] == 'obstacle' 99 | 100 | def test_exception_with_all_parameters(self) -> None: 101 | """Test exception with all parameters. 102 | 103 | Verifies that all parameters can be set together. 104 | """ 105 | details = {'param': 'value'} 106 | error = Nav2MCPError( 107 | 'Complete error', 108 | error_code=NavigationErrorCode.INVALID_PARAMETERS, 109 | details=details 110 | ) 111 | 112 | assert error.message == 'Complete error' 113 | assert error.error_code == NavigationErrorCode.INVALID_PARAMETERS 114 | assert error.details == details 115 | 116 | def test_to_dict_basic(self) -> None: 117 | """Test to_dict method with basic exception. 118 | 119 | Verifies that exception can be converted to dictionary. 120 | """ 121 | error = Nav2MCPError('Test error') 122 | error_dict = error.to_dict() 123 | 124 | assert isinstance(error_dict, dict) 125 | assert error_dict['error'] == 'Nav2MCPError' 126 | assert error_dict['message'] == 'Test error' 127 | assert error_dict['error_code'] == 'UNKNOWN' 128 | assert error_dict['details'] == {} 129 | 130 | def test_to_dict_with_details(self) -> None: 131 | """Test to_dict method with details. 132 | 133 | Verifies that details are included in dictionary. 134 | """ 135 | details = {'key': 'value', 'number': 42} 136 | error = Nav2MCPError( 137 | 'Error with details', 138 | error_code=NavigationErrorCode.TIMEOUT, 139 | details=details 140 | ) 141 | error_dict = error.to_dict() 142 | 143 | assert error_dict['message'] == 'Error with details' 144 | assert error_dict['error_code'] == 'TIMEOUT' 145 | assert error_dict['details'] == details 146 | 147 | def test_exception_is_raisable(self) -> None: 148 | """Test that exception can be raised and caught. 149 | 150 | Verifies that the exception works in try/except blocks. 151 | """ 152 | with pytest.raises(Nav2MCPError) as exc_info: 153 | raise Nav2MCPError('Raised error') 154 | 155 | assert str(exc_info.value) == 'Raised error' 156 | 157 | 158 | class TestNavigationError: 159 | """Tests for NavigationError exception class.""" 160 | 161 | def test_navigation_error_default_code(self) -> None: 162 | """Test NavigationError default error code. 163 | 164 | Verifies that NavigationError has appropriate default code. 165 | """ 166 | error = NavigationError('Navigation failed') 167 | 168 | assert error.error_code == NavigationErrorCode.NAVIGATION_FAILED 169 | 170 | def test_navigation_error_custom_code(self) -> None: 171 | """Test NavigationError with custom error code. 172 | 173 | Verifies that error code can be customized. 174 | """ 175 | error = NavigationError( 176 | 'Navigation canceled', 177 | error_code=NavigationErrorCode.NAVIGATION_CANCELED 178 | ) 179 | 180 | assert error.error_code == NavigationErrorCode.NAVIGATION_CANCELED 181 | 182 | def test_navigation_error_with_details(self) -> None: 183 | """Test NavigationError with details. 184 | 185 | Verifies that details can be attached to navigation errors. 186 | """ 187 | details = {'position': [1.0, 2.0], 'orientation': 0.5} 188 | error = NavigationError('Failed to reach goal', details=details) 189 | 190 | assert error.details == details 191 | 192 | def test_navigation_error_inheritance(self) -> None: 193 | """Test NavigationError inheritance. 194 | 195 | Verifies that NavigationError is a Nav2MCPError. 196 | """ 197 | error = NavigationError('Test') 198 | 199 | assert isinstance(error, Nav2MCPError) 200 | assert isinstance(error, Exception) 201 | 202 | def test_navigation_error_to_dict(self) -> None: 203 | """Test NavigationError to_dict method. 204 | 205 | Verifies dictionary representation includes correct class name. 206 | """ 207 | error = NavigationError('Nav error') 208 | error_dict = error.to_dict() 209 | 210 | assert error_dict['error'] == 'NavigationError' 211 | assert error_dict['error_code'] == 'NAVIGATION_FAILED' 212 | 213 | 214 | class TestTransformError: 215 | """Tests for TransformError exception class.""" 216 | 217 | def test_transform_error_default_code(self) -> None: 218 | """Test TransformError default error code. 219 | 220 | Verifies that TransformError has appropriate default code. 221 | """ 222 | error = TransformError('Transform unavailable') 223 | 224 | assert error.error_code == NavigationErrorCode.TRANSFORM_UNAVAILABLE 225 | 226 | def test_transform_error_custom_code(self) -> None: 227 | """Test TransformError with custom error code. 228 | 229 | Verifies that error code can be customized. 230 | """ 231 | error = TransformError( 232 | 'Transform timeout', 233 | error_code=NavigationErrorCode.TIMEOUT 234 | ) 235 | 236 | assert error.error_code == NavigationErrorCode.TIMEOUT 237 | 238 | def test_transform_error_with_details(self) -> None: 239 | """Test TransformError with frame details. 240 | 241 | Verifies that transform-specific details can be attached. 242 | """ 243 | details = {'source_frame': 'map', 'target_frame': 'base_link'} 244 | error = TransformError('Cannot transform', details=details) 245 | 246 | assert error.details['source_frame'] == 'map' 247 | assert error.details['target_frame'] == 'base_link' 248 | 249 | def test_transform_error_inheritance(self) -> None: 250 | """Test TransformError inheritance. 251 | 252 | Verifies that TransformError is a Nav2MCPError. 253 | """ 254 | error = TransformError('Test') 255 | 256 | assert isinstance(error, Nav2MCPError) 257 | 258 | def test_transform_error_to_dict(self) -> None: 259 | """Test TransformError to_dict method. 260 | 261 | Verifies dictionary representation. 262 | """ 263 | error = TransformError('Transform error') 264 | error_dict = error.to_dict() 265 | 266 | assert error_dict['error'] == 'TransformError' 267 | assert error_dict['error_code'] == 'TRANSFORM_UNAVAILABLE' 268 | 269 | 270 | class TestConfigurationError: 271 | """Tests for ConfigurationError exception class.""" 272 | 273 | def test_configuration_error_default_code(self) -> None: 274 | """Test ConfigurationError default error code. 275 | 276 | Verifies that ConfigurationError has appropriate default code. 277 | """ 278 | error = ConfigurationError('Invalid configuration') 279 | 280 | assert error.error_code == NavigationErrorCode.INVALID_PARAMETERS 281 | 282 | def test_configuration_error_with_details(self) -> None: 283 | """Test ConfigurationError with parameter details. 284 | 285 | Verifies that configuration-specific details can be attached. 286 | """ 287 | details = {'parameter': 'max_speed', 'value': -1, 'constraint': 'positive'} 288 | error = ConfigurationError('Invalid parameter', details=details) 289 | 290 | assert error.details == details 291 | 292 | def test_configuration_error_inheritance(self) -> None: 293 | """Test ConfigurationError inheritance. 294 | 295 | Verifies that ConfigurationError is a Nav2MCPError. 296 | """ 297 | error = ConfigurationError('Test') 298 | 299 | assert isinstance(error, Nav2MCPError) 300 | 301 | def test_configuration_error_to_dict(self) -> None: 302 | """Test ConfigurationError to_dict method. 303 | 304 | Verifies dictionary representation. 305 | """ 306 | error = ConfigurationError('Config error') 307 | error_dict = error.to_dict() 308 | 309 | assert error_dict['error'] == 'ConfigurationError' 310 | assert error_dict['error_code'] == 'INVALID_PARAMETERS' 311 | 312 | 313 | class TestTimeoutError: 314 | """Tests for TimeoutError exception class.""" 315 | 316 | def test_timeout_error_basic(self) -> None: 317 | """Test basic TimeoutError creation. 318 | 319 | Verifies that TimeoutError can be created without duration. 320 | """ 321 | error = TimeoutError('Operation timed out') 322 | 323 | assert error.message == 'Operation timed out' 324 | assert error.error_code == NavigationErrorCode.TIMEOUT 325 | assert error.details == {} 326 | 327 | def test_timeout_error_with_duration(self) -> None: 328 | """Test TimeoutError with timeout duration. 329 | 330 | Verifies that timeout duration is stored in details. 331 | """ 332 | error = TimeoutError('Timed out', timeout_duration=5.0) 333 | 334 | assert error.details['timeout_duration'] == 5.0 335 | 336 | def test_timeout_error_with_details_and_duration(self) -> None: 337 | """Test TimeoutError with both details and duration. 338 | 339 | Verifies that duration is added to existing details. 340 | """ 341 | details = {'operation': 'navigation'} 342 | error = TimeoutError( 343 | 'Navigation timeout', 344 | timeout_duration=10.0, 345 | details=details 346 | ) 347 | 348 | assert error.details['operation'] == 'navigation' 349 | assert error.details['timeout_duration'] == 10.0 350 | 351 | def test_timeout_error_without_duration_parameter(self) -> None: 352 | """Test TimeoutError when duration is None. 353 | 354 | Verifies that None duration is not added to details. 355 | """ 356 | error = TimeoutError('Timeout', timeout_duration=None) 357 | 358 | assert 'timeout_duration' not in error.details 359 | 360 | def test_timeout_error_inheritance(self) -> None: 361 | """Test TimeoutError inheritance. 362 | 363 | Verifies that TimeoutError is a Nav2MCPError. 364 | """ 365 | error = TimeoutError('Test') 366 | 367 | assert isinstance(error, Nav2MCPError) 368 | 369 | def test_timeout_error_to_dict(self) -> None: 370 | """Test TimeoutError to_dict method. 371 | 372 | Verifies dictionary representation includes timeout info. 373 | """ 374 | error = TimeoutError('Timeout', timeout_duration=3.5) 375 | error_dict = error.to_dict() 376 | 377 | assert error_dict['error'] == 'TimeoutError' 378 | assert error_dict['error_code'] == 'TIMEOUT' 379 | assert error_dict['details']['timeout_duration'] == 3.5 380 | 381 | 382 | class TestROSError: 383 | """Tests for ROSError exception class.""" 384 | 385 | def test_ros_error_basic(self) -> None: 386 | """Test basic ROSError creation. 387 | 388 | Verifies that ROSError can be created without ROS exception. 389 | """ 390 | error = ROSError('ROS operation failed') 391 | 392 | assert error.message == 'ROS operation failed' 393 | assert error.error_code == NavigationErrorCode.ROS_ERROR 394 | assert error.details == {} 395 | 396 | def test_ros_error_with_ros_exception(self) -> None: 397 | """Test ROSError with ROS exception details. 398 | 399 | Verifies that ROS exception info is stored in details. 400 | """ 401 | ros_exception = ValueError('ROS value error') 402 | error = ROSError('ROS failed', ros_exception=ros_exception) 403 | 404 | assert error.details['ros_exception_type'] == 'ValueError' 405 | assert error.details['ros_exception_message'] == 'ROS value error' 406 | 407 | def test_ros_error_with_details_and_exception(self) -> None: 408 | """Test ROSError with both details and ROS exception. 409 | 410 | Verifies that exception info is added to existing details. 411 | """ 412 | ros_exception = RuntimeError('Runtime issue') 413 | details = {'node': 'nav2_controller'} 414 | error = ROSError( 415 | 'Controller error', 416 | ros_exception=ros_exception, 417 | details=details 418 | ) 419 | 420 | assert error.details['node'] == 'nav2_controller' 421 | assert error.details['ros_exception_type'] == 'RuntimeError' 422 | assert error.details['ros_exception_message'] == 'Runtime issue' 423 | 424 | def test_ros_error_without_ros_exception(self) -> None: 425 | """Test ROSError when ros_exception is None. 426 | 427 | Verifies that None exception is not added to details. 428 | """ 429 | error = ROSError('ROS error', ros_exception=None) 430 | 431 | assert 'ros_exception_type' not in error.details 432 | assert 'ros_exception_message' not in error.details 433 | 434 | def test_ros_error_inheritance(self) -> None: 435 | """Test ROSError inheritance. 436 | 437 | Verifies that ROSError is a Nav2MCPError. 438 | """ 439 | error = ROSError('Test') 440 | 441 | assert isinstance(error, Nav2MCPError) 442 | 443 | def test_ros_error_to_dict(self) -> None: 444 | """Test ROSError to_dict method. 445 | 446 | Verifies dictionary representation includes ROS info. 447 | """ 448 | ros_exception = TypeError('Type mismatch') 449 | error = ROSError('ROS error', ros_exception=ros_exception) 450 | error_dict = error.to_dict() 451 | 452 | assert error_dict['error'] == 'ROSError' 453 | assert error_dict['error_code'] == 'ROS_ERROR' 454 | assert error_dict['details']['ros_exception_type'] == 'TypeError' 455 | 456 | 457 | class TestCreateNavigationErrorFromResult: 458 | """Tests for create_navigation_error_from_result utility function.""" 459 | 460 | def test_create_error_from_failed_result(self) -> None: 461 | """Test error creation from FAILED task result. 462 | 463 | Verifies that FAILED result creates appropriate error. 464 | """ 465 | # Mock TaskResult 466 | mock_task_result = Mock() 467 | mock_task_result.FAILED = 3 468 | mock_task_result.CANCELED = 2 469 | mock_task_result.SUCCEEDED = 1 470 | 471 | # Create a mock module 472 | import sys 473 | mock_module = Mock() 474 | mock_module.TaskResult = mock_task_result 475 | sys.modules['nav2_simple_commander'] = Mock() 476 | sys.modules['nav2_simple_commander.robot_navigator'] = mock_module 477 | 478 | try: 479 | error = create_navigation_error_from_result( 480 | mock_task_result.FAILED, 481 | 'Test operation' 482 | ) 483 | 484 | assert isinstance(error, NavigationError) 485 | assert error.error_code == NavigationErrorCode.NAVIGATION_FAILED 486 | assert 'Test operation failed' in error.message 487 | assert error.details['task_result'] == 'FAILED' 488 | finally: 489 | # Clean up mock 490 | if 'nav2_simple_commander' in sys.modules: 491 | del sys.modules['nav2_simple_commander'] 492 | if 'nav2_simple_commander.robot_navigator' in sys.modules: 493 | del sys.modules['nav2_simple_commander.robot_navigator'] 494 | 495 | def test_create_error_from_canceled_result(self) -> None: 496 | """Test error creation from CANCELED task result. 497 | 498 | Verifies that CANCELED result creates appropriate error. 499 | """ 500 | # Mock TaskResult 501 | mock_task_result = Mock() 502 | mock_task_result.FAILED = 3 503 | mock_task_result.CANCELED = 2 504 | mock_task_result.SUCCEEDED = 1 505 | 506 | import sys 507 | mock_module = Mock() 508 | mock_module.TaskResult = mock_task_result 509 | sys.modules['nav2_simple_commander'] = Mock() 510 | sys.modules['nav2_simple_commander.robot_navigator'] = mock_module 511 | 512 | try: 513 | error = create_navigation_error_from_result( 514 | mock_task_result.CANCELED, 515 | 'Test operation' 516 | ) 517 | 518 | assert isinstance(error, NavigationError) 519 | assert error.error_code == NavigationErrorCode.NAVIGATION_CANCELED 520 | assert 'Test operation was canceled' in error.message 521 | assert error.details['task_result'] == 'CANCELED' 522 | finally: 523 | # Clean up mock 524 | if 'nav2_simple_commander' in sys.modules: 525 | del sys.modules['nav2_simple_commander'] 526 | if 'nav2_simple_commander.robot_navigator' in sys.modules: 527 | del sys.modules['nav2_simple_commander.robot_navigator'] 528 | 529 | def test_create_error_from_unknown_result(self) -> None: 530 | """Test error creation from unknown task result. 531 | 532 | Verifies that unknown results create error with UNKNOWN code. 533 | """ 534 | # Mock TaskResult 535 | mock_task_result = Mock() 536 | mock_task_result.FAILED = 3 537 | mock_task_result.CANCELED = 2 538 | mock_task_result.SUCCEEDED = 1 539 | 540 | import sys 541 | mock_module = Mock() 542 | mock_module.TaskResult = mock_task_result 543 | sys.modules['nav2_simple_commander'] = Mock() 544 | sys.modules['nav2_simple_commander.robot_navigator'] = mock_module 545 | 546 | try: 547 | # Use a result value that's not FAILED or CANCELED 548 | unknown_result = 999 549 | error = create_navigation_error_from_result( 550 | unknown_result, 551 | 'Test operation' 552 | ) 553 | 554 | assert isinstance(error, NavigationError) 555 | assert error.error_code == NavigationErrorCode.UNKNOWN 556 | assert 'unexpected result' in error.message.lower() 557 | assert '999' in error.details['task_result'] 558 | finally: 559 | # Clean up mock 560 | if 'nav2_simple_commander' in sys.modules: 561 | del sys.modules['nav2_simple_commander'] 562 | if 'nav2_simple_commander.robot_navigator' in sys.modules: 563 | del sys.modules['nav2_simple_commander.robot_navigator'] 564 | 565 | 566 | class TestExceptionInteroperability: 567 | """Tests for exception interoperability and edge cases.""" 568 | 569 | def test_exception_can_be_caught_as_base_exception(self) -> None: 570 | """Test that specific exceptions can be caught as base type. 571 | 572 | Verifies exception hierarchy works correctly. 573 | """ 574 | with pytest.raises(Nav2MCPError): 575 | raise NavigationError('Test') 576 | 577 | with pytest.raises(Nav2MCPError): 578 | raise TransformError('Test') 579 | 580 | with pytest.raises(Nav2MCPError): 581 | raise ConfigurationError('Test') 582 | 583 | def test_exception_message_persistence(self) -> None: 584 | """Test that exception messages persist through raise/catch. 585 | 586 | Verifies message is preserved in exception handling. 587 | """ 588 | original_message = 'Persistent error message' 589 | 590 | with pytest.raises(Nav2MCPError) as exc_info: 591 | raise Nav2MCPError(original_message) 592 | 593 | assert exc_info.value.message == original_message 594 | assert str(exc_info.value) == original_message 595 | 596 | def test_exception_details_mutability(self) -> None: 597 | """Test exception details dictionary mutability. 598 | 599 | Verifies that details can be modified after creation. 600 | """ 601 | error = Nav2MCPError('Test', details={'key': 'value'}) 602 | 603 | # Details should be mutable 604 | error.details['new_key'] = 'new_value' 605 | 606 | assert error.details['key'] == 'value' 607 | assert error.details['new_key'] == 'new_value' 608 | 609 | def test_multiple_exceptions_independent(self) -> None: 610 | """Test that multiple exception instances are independent. 611 | 612 | Verifies that exception instances don't share state. 613 | """ 614 | error1 = Nav2MCPError('Error 1', details={'id': 1}) 615 | error2 = Nav2MCPError('Error 2', details={'id': 2}) 616 | 617 | assert error1.details['id'] == 1 618 | assert error2.details['id'] == 2 619 | assert error1.message != error2.message 620 | -------------------------------------------------------------------------------- /tests/test_resources.py: -------------------------------------------------------------------------------- 1 | """Tests for Nav2 MCP Server resources. 2 | 3 | This module tests all resource endpoints including robot pose 4 | information and error handling. 5 | """ 6 | 7 | from unittest.mock import Mock, patch 8 | 9 | from fastmcp import Client 10 | 11 | from nav2_mcp_server.server import create_server 12 | from tests.conftest import create_mock_config 13 | 14 | 15 | class TestRobotPoseResource: 16 | """Tests for robot pose resource endpoint.""" 17 | 18 | async def test_robot_pose_resource_success( 19 | self, 20 | mock_transform_manager: Mock 21 | ) -> None: 22 | """Test successful robot pose resource retrieval. 23 | 24 | Verifies that the resource endpoint correctly returns 25 | robot pose information in JSON format. 26 | """ 27 | with patch('nav2_mcp_server.server.get_config', return_value=create_mock_config()): 28 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 29 | with patch('nav2_mcp_server.tools.get_transform_manager'): 30 | with patch( 31 | 'nav2_mcp_server.resources.get_transform_manager', 32 | return_value=mock_transform_manager 33 | ): 34 | server = create_server() 35 | async with Client(server) as client: 36 | resources = await client.list_resources() 37 | assert len(resources) > 0 38 | 39 | # Get the robot pose resource 40 | pose_resource = None 41 | for resource in resources: 42 | if 'pose' in str(resource.uri).lower(): 43 | pose_resource = resource 44 | break 45 | 46 | assert pose_resource is not None 47 | assert pose_resource.mimeType == 'application/json' 48 | 49 | async def test_robot_pose_resource_content( 50 | self, 51 | mock_transform_manager: Mock 52 | ) -> None: 53 | """Test robot pose resource returns correct content. 54 | 55 | Verifies that the resource content matches expected 56 | pose data structure. 57 | """ 58 | expected_pose = { 59 | 'pose': { 60 | 'position': {'x': 1.0, 'y': 2.0, 'z': 0.0}, 61 | 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0} 62 | }, 63 | 'header': { 64 | 'frame_id': 'map', 65 | 'stamp': {'sec': 1640995200, 'nanosec': 0} 66 | }, 67 | 'yaw': 0.0, 68 | 'status': 'success' 69 | } 70 | 71 | mock_transform_manager.get_robot_pose.return_value = expected_pose 72 | 73 | with patch('nav2_mcp_server.server.get_config', return_value=create_mock_config()): 74 | with patch('nav2_mcp_server.resources.get_config', return_value=create_mock_config()): 75 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 76 | with patch('nav2_mcp_server.tools.get_transform_manager'): 77 | with patch( 78 | 'nav2_mcp_server.resources.get_transform_manager', 79 | return_value=mock_transform_manager 80 | ): 81 | server = create_server() 82 | async with Client(server) as client: 83 | # Get resource content 84 | resource_contents = await client.read_resource( 85 | 'nav2://robot_pose' 86 | ) 87 | 88 | assert resource_contents is not None 89 | assert len(resource_contents) > 0 90 | resource_content = resource_contents[0] 91 | 92 | # Content should be in JSON format 93 | import json 94 | parsed_content = json.loads(resource_content.text) 95 | assert 'pose' in parsed_content 96 | assert 'status' in parsed_content 97 | 98 | async def test_robot_pose_resource_error_handling(self) -> None: 99 | """Test robot pose resource handles errors gracefully. 100 | 101 | Verifies that the resource returns error information 102 | when transform lookup fails. 103 | """ 104 | mock_tf_manager = Mock() 105 | mock_tf_manager.get_robot_pose.side_effect = Exception( 106 | 'Transform lookup failed' 107 | ) 108 | 109 | with patch('nav2_mcp_server.server.get_config', return_value=create_mock_config()): 110 | with patch('nav2_mcp_server.resources.get_config', return_value=create_mock_config()): 111 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 112 | with patch('nav2_mcp_server.tools.get_transform_manager'): 113 | with patch( 114 | 'nav2_mcp_server.resources.get_transform_manager', 115 | return_value=mock_tf_manager 116 | ): 117 | server = create_server() 118 | async with Client(server) as client: 119 | # Get resource content with error 120 | resource_contents = await client.read_resource( 121 | 'nav2://robot_pose' 122 | ) 123 | 124 | assert resource_contents is not None 125 | assert len(resource_contents) > 0 126 | resource_content = resource_contents[0] 127 | 128 | # Content should contain error information 129 | import json 130 | parsed_content = json.loads(resource_content.text) 131 | assert 'error' in parsed_content 132 | assert 'message' in parsed_content 133 | 134 | async def test_robot_pose_resource_mime_type(self) -> None: 135 | """Test robot pose resource has correct MIME type. 136 | 137 | Verifies that the resource is properly configured 138 | with JSON MIME type. 139 | """ 140 | with patch('nav2_mcp_server.server.get_config', return_value=create_mock_config()): 141 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 142 | with patch('nav2_mcp_server.tools.get_transform_manager'): 143 | with patch( 144 | 'nav2_mcp_server.resources.get_transform_manager' 145 | ): 146 | server = create_server() 147 | async with Client(server) as client: 148 | resources = await client.list_resources() 149 | 150 | # Find the robot pose resource 151 | pose_resource = None 152 | for resource in resources: 153 | if 'robot pose' in resource.name.lower(): 154 | pose_resource = resource 155 | break 156 | 157 | assert pose_resource is not None 158 | assert pose_resource.mimeType == 'application/json' 159 | assert 'robot pose' in resource.description.lower() 160 | 161 | 162 | class TestResourceErrorHandling: 163 | """Tests for resource error handling scenarios.""" 164 | 165 | async def test_resource_with_manager_unavailable(self) -> None: 166 | """Test resource behavior when manager is unavailable. 167 | 168 | Verifies that resources handle manager initialization 169 | failures gracefully. 170 | """ 171 | with patch('nav2_mcp_server.server.get_config', return_value=create_mock_config()): 172 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 173 | with patch('nav2_mcp_server.tools.get_transform_manager'): 174 | with patch( 175 | 'nav2_mcp_server.resources.get_transform_manager', 176 | side_effect=Exception('Manager unavailable') 177 | ): 178 | server = create_server() 179 | async with Client(server) as client: 180 | # Should still be able to list resources 181 | resources = await client.list_resources() 182 | assert isinstance(resources, list) 183 | 184 | async def test_resource_json_serialization(self) -> None: 185 | """Test resource handles complex data serialization. 186 | 187 | Verifies that the resource can handle various data types 188 | in the pose information. 189 | """ 190 | mock_tf_manager = Mock() 191 | complex_pose = { 192 | 'pose': { 193 | 'position': {'x': 1.5, 'y': -2.3, 'z': 0.1}, 194 | 'orientation': { 195 | 'x': 0.0, 'y': 0.0, 'z': 0.707, 'w': 0.707 196 | } 197 | }, 198 | 'header': { 199 | 'frame_id': 'map', 200 | 'stamp': {'sec': 1640995200, 'nanosec': 123456789} 201 | }, 202 | 'yaw': 1.5708, 203 | 'status': 'success', 204 | 'additional_info': { 205 | 'confidence': 0.95, 206 | 'source': 'amcl' 207 | } 208 | } 209 | 210 | mock_tf_manager.get_robot_pose.return_value = complex_pose 211 | 212 | with patch('nav2_mcp_server.server.get_config', return_value=create_mock_config()): 213 | with patch('nav2_mcp_server.resources.get_config', return_value=create_mock_config()): 214 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 215 | with patch('nav2_mcp_server.tools.get_transform_manager'): 216 | with patch( 217 | 'nav2_mcp_server.resources.get_transform_manager', 218 | return_value=mock_tf_manager 219 | ): 220 | server = create_server() 221 | async with Client(server) as client: 222 | # Get resource content 223 | resource_contents = await client.read_resource( 224 | 'nav2://robot_pose' 225 | ) 226 | 227 | assert resource_contents is not None 228 | assert len(resource_contents) > 0 229 | resource_content = resource_contents[0] 230 | 231 | # Should be able to parse complex JSON 232 | import json 233 | parsed_content = json.loads(resource_content.text) 234 | assert parsed_content['yaw'] == 1.5708 235 | assert 'additional_info' in parsed_content 236 | assert parsed_content['additional_info']['confidence'] == 0.95 237 | -------------------------------------------------------------------------------- /tests/test_server.py: -------------------------------------------------------------------------------- 1 | """Tests for the main Nav2 MCP Server module. 2 | 3 | This module tests the server initialization, tool registration, 4 | and basic server functionality. 5 | """ 6 | 7 | from unittest.mock import AsyncMock, Mock, patch 8 | 9 | from fastmcp import Client 10 | 11 | from nav2_mcp_server.server import create_server 12 | 13 | 14 | async def test_server_initialization() -> None: 15 | """Test that the server initializes correctly. 16 | 17 | Verifies that the FastMCP server instance is created with 18 | the correct name and configuration. 19 | """ 20 | with patch('nav2_mcp_server.server.get_config') as mock_config: 21 | mock_config.return_value.server.server_name = 'Nav2 MCP' 22 | 23 | server = create_server() 24 | assert server.name == 'Nav2 MCP' 25 | assert server is not None 26 | 27 | 28 | async def test_server_has_tools() -> None: 29 | """Test that the server has tools registered. 30 | 31 | Verifies that tools are properly registered during server 32 | initialization. 33 | """ 34 | with patch('nav2_mcp_server.server.get_config'): 35 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 36 | with patch('nav2_mcp_server.tools.get_transform_manager'): 37 | server = create_server() 38 | tools = await server.get_tools() 39 | assert len(tools) > 0, ( 40 | 'Server should have at least one tool registered' 41 | ) 42 | 43 | 44 | async def test_server_tool_names() -> None: 45 | """Test that expected tools are registered with correct names. 46 | 47 | Verifies that all core navigation tools are present in the server. 48 | """ 49 | with patch('nav2_mcp_server.server.get_config'): 50 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 51 | with patch('nav2_mcp_server.tools.get_transform_manager'): 52 | server = create_server() 53 | tools = await server.get_tools() 54 | 55 | # get_tools() returns a dict mapping names to tool definitions 56 | if isinstance(tools, dict): 57 | tool_names = list(tools.keys()) 58 | elif isinstance(tools, list): 59 | tool_names = [ 60 | t if isinstance(t, str) else t.name for t in tools 61 | ] 62 | else: 63 | tool_names = [] 64 | 65 | expected_tools = [ 66 | 'navigate_to_pose', 67 | 'follow_waypoints', 68 | 'spin_robot', 69 | 'backup_robot', 70 | 'dock_robot', 71 | 'undock_robot', 72 | 'get_path', 73 | 'get_path_from_robot', 74 | 'clear_costmaps', 75 | 'get_robot_pose', 76 | 'cancel_navigation', 77 | 'nav2_lifecycle', 78 | ] 79 | 80 | for expected_tool in expected_tools: 81 | assert expected_tool in tool_names, ( 82 | f"Tool '{expected_tool}' should be registered" 83 | ) 84 | 85 | 86 | async def test_server_has_resources() -> None: 87 | """Test that the server has resources registered. 88 | 89 | Verifies that resources are properly registered during server 90 | initialization. 91 | """ 92 | with patch('nav2_mcp_server.server.get_config'): 93 | with patch('nav2_mcp_server.resources.get_transform_manager'): 94 | server = create_server() 95 | resources = await server.get_resources() 96 | assert len(resources) > 0, ( 97 | 'Server should have at least one resource registered' 98 | ) 99 | 100 | 101 | async def test_server_client_connection() -> None: 102 | """Test that a client can connect to the server. 103 | 104 | Verifies that the in-memory transport works correctly and 105 | the client can establish a connection. 106 | """ 107 | from tests.conftest import create_mock_config 108 | 109 | with patch('nav2_mcp_server.server.get_config', return_value=create_mock_config()): 110 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 111 | with patch('nav2_mcp_server.tools.get_transform_manager'): 112 | with patch('nav2_mcp_server.resources.get_transform_manager'): 113 | server = create_server() 114 | async with Client(server) as client: 115 | # Test that client is connected 116 | result = await client.ping() 117 | assert result is True, ( 118 | 'Client should be able to ping server' 119 | ) 120 | 121 | 122 | async def test_server_list_tools_via_client() -> None: 123 | """Test that tools can be listed through a client connection. 124 | 125 | Verifies that the MCP protocol correctly exposes available tools. 126 | """ 127 | from tests.conftest import create_mock_config 128 | 129 | with patch('nav2_mcp_server.server.get_config', return_value=create_mock_config()): 130 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 131 | with patch('nav2_mcp_server.tools.get_transform_manager'): 132 | with patch('nav2_mcp_server.resources.get_transform_manager'): 133 | server = create_server() 134 | async with Client(server) as client: 135 | tools_response = await client.list_tools() 136 | assert len(tools_response) > 0, ( 137 | 'Client should receive list of available tools' 138 | ) 139 | 140 | 141 | async def test_server_list_resources_via_client() -> None: 142 | """Test that resources can be listed through a client connection. 143 | 144 | Verifies that the MCP protocol correctly exposes available resources. 145 | """ 146 | from tests.conftest import create_mock_config 147 | 148 | with patch('nav2_mcp_server.server.get_config', return_value=create_mock_config()): 149 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 150 | with patch('nav2_mcp_server.tools.get_transform_manager'): 151 | with patch('nav2_mcp_server.resources.get_transform_manager'): 152 | server = create_server() 153 | async with Client(server) as client: 154 | resources_response = await client.list_resources() 155 | assert len(resources_response) > 0, ( 156 | 'Client should receive list of available resources' 157 | ) 158 | 159 | 160 | async def test_main_function_success() -> None: 161 | """Test the main function executes successfully. 162 | 163 | Verifies that the main function initializes ROS2, creates managers, 164 | and starts the server. 165 | """ 166 | from nav2_mcp_server.server import main 167 | 168 | mock_server = Mock() 169 | mock_server.run_async = AsyncMock() 170 | mock_nav_manager = Mock() 171 | mock_nav_manager.destroy = Mock() 172 | mock_transform_manager = Mock() 173 | mock_transform_manager.destroy = Mock() 174 | mock_logger = Mock() 175 | 176 | with patch('nav2_mcp_server.server.setup_logging', return_value=mock_logger): 177 | with patch('nav2_mcp_server.server.rclpy.init') as mock_init: 178 | with patch( 179 | 'nav2_mcp_server.server.get_navigation_manager', 180 | return_value=mock_nav_manager 181 | ): 182 | with patch( 183 | 'nav2_mcp_server.server.get_transform_manager', 184 | return_value=mock_transform_manager 185 | ): 186 | with patch( 187 | 'nav2_mcp_server.server.create_server', 188 | return_value=mock_server 189 | ): 190 | with patch( 191 | 'nav2_mcp_server.server.rclpy.shutdown' 192 | ) as mock_shutdown: 193 | # Run main 194 | await main() 195 | 196 | # Verify initialization 197 | mock_logger.info.assert_any_call( 198 | 'Starting Nav2 MCP Server...' 199 | ) 200 | mock_init.assert_called_once() 201 | mock_logger.info.assert_any_call( 202 | 'ROS2 initialized' 203 | ) 204 | mock_logger.info.assert_any_call( 205 | 'Navigation and transform managers initialized' 206 | ) 207 | mock_logger.info.assert_any_call( 208 | 'Starting MCP server on stdio transport' 209 | ) 210 | 211 | # Verify cleanup 212 | mock_logger.info.assert_any_call( 213 | 'Shutting down ROS2...' 214 | ) 215 | mock_nav_manager.destroy.assert_called_once() 216 | mock_transform_manager.destroy.assert_called_once() 217 | mock_shutdown.assert_called_once() 218 | mock_logger.info.assert_any_call( 219 | 'Nav2 MCP Server shutdown complete' 220 | ) 221 | 222 | 223 | async def test_main_function_keyboard_interrupt() -> None: 224 | """Test the main function handles KeyboardInterrupt gracefully. 225 | 226 | Verifies that the server shuts down properly when interrupted. 227 | """ 228 | from nav2_mcp_server.server import main 229 | 230 | mock_server = Mock() 231 | mock_server.run_async = AsyncMock(side_effect=KeyboardInterrupt()) 232 | mock_nav_manager = Mock() 233 | mock_nav_manager.destroy = Mock() 234 | mock_transform_manager = Mock() 235 | mock_transform_manager.destroy = Mock() 236 | mock_logger = Mock() 237 | 238 | with patch('nav2_mcp_server.server.setup_logging', return_value=mock_logger): 239 | with patch('nav2_mcp_server.server.rclpy.init'): 240 | with patch( 241 | 'nav2_mcp_server.server.get_navigation_manager', 242 | return_value=mock_nav_manager 243 | ): 244 | with patch( 245 | 'nav2_mcp_server.server.get_transform_manager', 246 | return_value=mock_transform_manager 247 | ): 248 | with patch( 249 | 'nav2_mcp_server.server.create_server', 250 | return_value=mock_server 251 | ): 252 | with patch( 253 | 'nav2_mcp_server.server.rclpy.shutdown' 254 | ) as mock_shutdown: 255 | # Run main 256 | await main() 257 | 258 | # Verify interrupt handling 259 | mock_logger.info.assert_any_call( 260 | 'Server interrupted by user' 261 | ) 262 | mock_shutdown.assert_called_once() 263 | 264 | 265 | async def test_main_function_exception() -> None: 266 | """Test the main function handles exceptions properly. 267 | 268 | Verifies that exceptions are logged and re-raised. 269 | """ 270 | import pytest 271 | 272 | from nav2_mcp_server.server import main 273 | 274 | mock_server = Mock() 275 | mock_server.run_async = AsyncMock(side_effect=RuntimeError('Test error')) 276 | mock_nav_manager = Mock() 277 | mock_nav_manager.destroy = Mock() 278 | mock_transform_manager = Mock() 279 | mock_transform_manager.destroy = Mock() 280 | mock_logger = Mock() 281 | 282 | with patch('nav2_mcp_server.server.setup_logging', return_value=mock_logger): 283 | with patch('nav2_mcp_server.server.rclpy.init'): 284 | with patch( 285 | 'nav2_mcp_server.server.get_navigation_manager', 286 | return_value=mock_nav_manager 287 | ): 288 | with patch( 289 | 'nav2_mcp_server.server.get_transform_manager', 290 | return_value=mock_transform_manager 291 | ): 292 | with patch( 293 | 'nav2_mcp_server.server.create_server', 294 | return_value=mock_server 295 | ): 296 | with patch( 297 | 'nav2_mcp_server.server.rclpy.shutdown' 298 | ) as mock_shutdown: 299 | # Run main and expect exception 300 | with pytest.raises( 301 | RuntimeError, match='Test error' 302 | ): 303 | await main() 304 | 305 | # Verify error handling 306 | mock_logger.error.assert_any_call( 307 | 'Server error: Test error' 308 | ) 309 | mock_shutdown.assert_called_once() 310 | 311 | 312 | async def test_main_function_cleanup_without_destroy() -> None: 313 | """Test the main function cleanup when managers don't have destroy method. 314 | 315 | Verifies that cleanup works even if managers lack destroy method. 316 | """ 317 | from nav2_mcp_server.server import main 318 | 319 | mock_server = Mock() 320 | mock_server.run_async = AsyncMock() 321 | # Create managers without destroy method 322 | mock_nav_manager = Mock(spec=[]) 323 | mock_transform_manager = Mock(spec=[]) 324 | mock_logger = Mock() 325 | 326 | with patch('nav2_mcp_server.server.setup_logging', return_value=mock_logger): 327 | with patch('nav2_mcp_server.server.rclpy.init'): 328 | with patch( 329 | 'nav2_mcp_server.server.get_navigation_manager', 330 | return_value=mock_nav_manager 331 | ): 332 | with patch( 333 | 'nav2_mcp_server.server.get_transform_manager', 334 | return_value=mock_transform_manager 335 | ): 336 | with patch( 337 | 'nav2_mcp_server.server.create_server', 338 | return_value=mock_server 339 | ): 340 | with patch( 341 | 'nav2_mcp_server.server.rclpy.shutdown' 342 | ) as mock_shutdown: 343 | # Run main 344 | await main() 345 | 346 | # Verify cleanup still works 347 | mock_shutdown.assert_called_once() 348 | mock_logger.info.assert_any_call( 349 | 'Nav2 MCP Server shutdown complete' 350 | ) 351 | -------------------------------------------------------------------------------- /tests/test_tools.py: -------------------------------------------------------------------------------- 1 | """Tests for Nav2 MCP Server tools. 2 | 3 | This module tests all tool implementations including error handling, 4 | parameter validation, and correct data processing. 5 | """ 6 | 7 | import json 8 | from typing import Any, Dict, List 9 | from unittest.mock import Mock, patch 10 | 11 | import pytest 12 | from fastmcp import Client, FastMCP 13 | 14 | 15 | class TestNavigateToPose: 16 | """Tests for navigate_to_pose tool.""" 17 | 18 | async def test_navigate_to_pose_success( 19 | self, 20 | test_server: FastMCP, 21 | mock_navigation_manager: Mock, 22 | sample_pose: Dict[str, Any] 23 | ) -> None: 24 | """Test successful navigation to pose.""" 25 | with patch( 26 | 'nav2_mcp_server.tools.get_navigation_manager', 27 | return_value=mock_navigation_manager 28 | ): 29 | with patch('nav2_mcp_server.tools.get_transform_manager'): 30 | async with Client(test_server) as client: 31 | result = await client.call_tool( 32 | 'navigate_to_pose', 33 | { 34 | 'x': sample_pose['position']['x'], 35 | 'y': sample_pose['position']['y'], 36 | 'yaw': 1.57 37 | } 38 | ) 39 | 40 | assert result.content 41 | mock_navigation_manager.navigate_to_pose.assert_called_once() 42 | 43 | async def test_navigate_to_pose_invalid_coordinates( 44 | self, 45 | test_server: FastMCP 46 | ) -> None: 47 | """Test navigation with invalid coordinates.""" 48 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 49 | with patch('nav2_mcp_server.tools.get_transform_manager'): 50 | async with Client(test_server) as client: 51 | with pytest.raises(Exception): 52 | await client.call_tool( 53 | 'navigate_to_pose', 54 | { 55 | 'x': 'invalid', 56 | 'y': 2.0, 57 | 'yaw': 1.57 58 | } 59 | ) 60 | 61 | 62 | class TestFollowWaypoints: 63 | """Tests for follow_waypoints tool.""" 64 | 65 | async def test_follow_waypoints_success( 66 | self, 67 | test_server: FastMCP, 68 | mock_navigation_manager: Mock 69 | ) -> None: 70 | """Test successful waypoint following.""" 71 | waypoints_str = '[[1.0, 2.0], [3.0, 4.0]]' 72 | 73 | with patch( 74 | 'nav2_mcp_server.tools.get_navigation_manager', 75 | return_value=mock_navigation_manager 76 | ): 77 | with patch('nav2_mcp_server.tools.get_transform_manager'): 78 | async with Client(test_server) as client: 79 | result = await client.call_tool( 80 | 'follow_waypoints', 81 | {'waypoints': waypoints_str} 82 | ) 83 | 84 | assert result.content 85 | mock_navigation_manager.follow_waypoints.assert_called_once() 86 | 87 | async def test_follow_waypoints_empty_list( 88 | self, 89 | test_server: FastMCP, 90 | mock_navigation_manager: Mock 91 | ) -> None: 92 | """Test waypoint following with empty waypoint list.""" 93 | with patch( 94 | 'nav2_mcp_server.tools.get_navigation_manager', 95 | return_value=mock_navigation_manager 96 | ): 97 | with patch('nav2_mcp_server.tools.get_transform_manager'): 98 | async with Client(test_server) as client: 99 | result = await client.call_tool( 100 | 'follow_waypoints', 101 | {'waypoints': '[]'} 102 | ) 103 | 104 | assert result.content 105 | 106 | 107 | class TestSpinRobot: 108 | """Tests for spin_robot tool.""" 109 | 110 | async def test_spin_robot_success( 111 | self, 112 | test_server: FastMCP, 113 | mock_navigation_manager: Mock 114 | ) -> None: 115 | """Test successful robot spinning.""" 116 | with patch( 117 | 'nav2_mcp_server.tools.get_navigation_manager', 118 | return_value=mock_navigation_manager 119 | ): 120 | with patch('nav2_mcp_server.tools.get_transform_manager'): 121 | async with Client(test_server) as client: 122 | result = await client.call_tool( 123 | 'spin_robot', 124 | {'angle': 1.57} 125 | ) 126 | 127 | assert result.content 128 | mock_navigation_manager.spin_robot.assert_called_once() 129 | 130 | async def test_spin_robot_invalid_angle( 131 | self, 132 | test_server: FastMCP 133 | ) -> None: 134 | """Test robot spinning with invalid angle.""" 135 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 136 | with patch('nav2_mcp_server.tools.get_transform_manager'): 137 | async with Client(test_server) as client: 138 | with pytest.raises(Exception): 139 | await client.call_tool( 140 | 'spin_robot', 141 | {'angular_distance': 'invalid'} 142 | ) 143 | 144 | 145 | class TestBackupRobot: 146 | """Tests for backup_robot tool.""" 147 | 148 | async def test_backup_robot_success( 149 | self, 150 | test_server: FastMCP, 151 | mock_navigation_manager: Mock 152 | ) -> None: 153 | """Test successful robot backup.""" 154 | with patch( 155 | 'nav2_mcp_server.tools.get_navigation_manager', 156 | return_value=mock_navigation_manager 157 | ): 158 | with patch('nav2_mcp_server.tools.get_transform_manager'): 159 | async with Client(test_server) as client: 160 | result = await client.call_tool( 161 | 'backup_robot', 162 | { 163 | 'distance': 1.0, 164 | 'speed': 0.15 165 | } 166 | ) 167 | 168 | assert result.content 169 | mock_navigation_manager.backup_robot.assert_called_once() 170 | 171 | async def test_backup_robot_default_speed( 172 | self, 173 | test_server: FastMCP, 174 | mock_navigation_manager: Mock 175 | ) -> None: 176 | """Test robot backup with default speed (speed parameter = 0.0). 177 | 178 | Verifies that when speed is 0.0, the default backup speed from 179 | config is used. 180 | """ 181 | with patch( 182 | 'nav2_mcp_server.tools.get_navigation_manager', 183 | return_value=mock_navigation_manager 184 | ): 185 | with patch('nav2_mcp_server.tools.get_transform_manager'): 186 | with patch('nav2_mcp_server.tools.get_config') as mock_config: 187 | # Set default backup speed in config 188 | config_obj = Mock() 189 | config_obj.navigation.default_backup_speed = 0.25 190 | mock_config.return_value = config_obj 191 | 192 | async with Client(test_server) as client: 193 | result = await client.call_tool( 194 | 'backup_robot', 195 | { 196 | 'distance': 1.0 197 | # speed not specified, should use default 198 | } 199 | ) 200 | 201 | assert result.content 202 | mock_navigation_manager.backup_robot.assert_called_once() 203 | 204 | 205 | class TestDockRobot: 206 | """Tests for dock_robot tool.""" 207 | 208 | async def test_dock_robot_success( 209 | self, 210 | test_server: FastMCP, 211 | mock_navigation_manager: Mock 212 | ) -> None: 213 | """Test successful robot docking.""" 214 | with patch( 215 | 'nav2_mcp_server.tools.get_navigation_manager', 216 | return_value=mock_navigation_manager 217 | ): 218 | with patch('nav2_mcp_server.tools.get_transform_manager'): 219 | async with Client(test_server) as client: 220 | result = await client.call_tool( 221 | 'dock_robot', 222 | { 223 | 'x': 5.0, 224 | 'y': 2.0, 225 | 'yaw': 0.0 226 | } 227 | ) 228 | 229 | assert result.content 230 | mock_navigation_manager.dock_robot.assert_called_once() 231 | 232 | 233 | class TestUndockRobot: 234 | """Tests for undock_robot tool.""" 235 | 236 | async def test_undock_robot_success( 237 | self, 238 | test_server: FastMCP, 239 | mock_navigation_manager: Mock 240 | ) -> None: 241 | """Test successful robot undocking.""" 242 | with patch( 243 | 'nav2_mcp_server.tools.get_navigation_manager', 244 | return_value=mock_navigation_manager 245 | ): 246 | with patch('nav2_mcp_server.tools.get_transform_manager'): 247 | async with Client(test_server) as client: 248 | result = await client.call_tool( 249 | 'undock_robot', 250 | {'dock_type': 'charging_dock'} 251 | ) 252 | 253 | assert result.content 254 | mock_navigation_manager.undock_robot.assert_called_once() 255 | 256 | 257 | class TestGetPath: 258 | """Tests for get_path tool.""" 259 | 260 | async def test_get_path_success( 261 | self, 262 | test_server: FastMCP, 263 | mock_navigation_manager: Mock, 264 | sample_path: Dict[str, Any] 265 | ) -> None: 266 | """Test successful path planning.""" 267 | mock_navigation_manager.get_path.return_value = json.dumps(sample_path) 268 | 269 | with patch( 270 | 'nav2_mcp_server.tools.get_navigation_manager', 271 | return_value=mock_navigation_manager 272 | ): 273 | with patch('nav2_mcp_server.tools.get_transform_manager'): 274 | async with Client(test_server) as client: 275 | result = await client.call_tool( 276 | 'get_path', 277 | { 278 | 'start_x': 0.0, 279 | 'start_y': 0.0, 280 | 'start_yaw': 0.0, 281 | 'goal_x': 2.0, 282 | 'goal_y': 2.0, 283 | 'goal_yaw': 1.57, 284 | 'planner_id': 'GridBased' 285 | } 286 | ) 287 | 288 | assert result.content 289 | mock_navigation_manager.get_path.assert_called_once() 290 | 291 | 292 | class TestGetPathFromRobot: 293 | """Tests for get_path_from_robot tool.""" 294 | 295 | async def test_get_path_from_robot_success( 296 | self, 297 | test_server: FastMCP, 298 | mock_navigation_manager: Mock, 299 | sample_path: Dict[str, Any] 300 | ) -> None: 301 | """Test successful path planning from robot position.""" 302 | # get_path_from_robot internally calls get_path 303 | mock_navigation_manager.get_path.return_value = json.dumps(sample_path) 304 | 305 | with patch( 306 | 'nav2_mcp_server.tools.get_navigation_manager', 307 | return_value=mock_navigation_manager 308 | ): 309 | with patch('nav2_mcp_server.tools.get_transform_manager'): 310 | async with Client(test_server) as client: 311 | result = await client.call_tool( 312 | 'get_path_from_robot', 313 | { 314 | 'goal_x': 2.0, 315 | 'goal_y': 2.0, 316 | 'goal_yaw': 1.57, 317 | 'planner_id': 'GridBased' 318 | } 319 | ) 320 | 321 | assert result.content 322 | mock_navigation_manager.get_path.assert_called_once() 323 | 324 | 325 | class TestClearCostmaps: 326 | """Tests for clear_costmaps tool.""" 327 | 328 | async def test_clear_costmaps_success( 329 | self, 330 | test_server: FastMCP, 331 | mock_navigation_manager: Mock 332 | ) -> None: 333 | """Test successful costmap clearing.""" 334 | with patch( 335 | 'nav2_mcp_server.tools.get_navigation_manager', 336 | return_value=mock_navigation_manager 337 | ): 338 | with patch('nav2_mcp_server.tools.get_transform_manager'): 339 | async with Client(test_server) as client: 340 | result = await client.call_tool( 341 | 'clear_costmaps', 342 | {} 343 | ) 344 | 345 | assert result.content 346 | mock_navigation_manager.clear_costmaps.assert_called_once() 347 | 348 | 349 | class TestGetRobotPose: 350 | """Tests for get_robot_pose tool.""" 351 | 352 | async def test_get_robot_pose_success( 353 | self, 354 | test_server: FastMCP, 355 | mock_transform_manager: Mock 356 | ) -> None: 357 | """Test successful robot pose retrieval.""" 358 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 359 | with patch( 360 | 'nav2_mcp_server.tools.get_transform_manager', 361 | return_value=mock_transform_manager 362 | ): 363 | async with Client(test_server) as client: 364 | result = await client.call_tool( 365 | 'get_robot_pose', 366 | {} 367 | ) 368 | 369 | assert result.content 370 | mock_transform_manager.get_robot_pose.assert_called_once() 371 | 372 | 373 | class TestCancelNavigation: 374 | """Tests for cancel_navigation tool.""" 375 | 376 | async def test_cancel_navigation_success( 377 | self, 378 | test_server: FastMCP, 379 | mock_navigation_manager: Mock 380 | ) -> None: 381 | """Test successful navigation cancellation.""" 382 | with patch( 383 | 'nav2_mcp_server.tools.get_navigation_manager', 384 | return_value=mock_navigation_manager 385 | ): 386 | with patch('nav2_mcp_server.tools.get_transform_manager'): 387 | async with Client(test_server) as client: 388 | result = await client.call_tool( 389 | 'cancel_navigation', 390 | {} 391 | ) 392 | 393 | assert result.content 394 | mock_navigation_manager.cancel_navigation.assert_called_once() 395 | 396 | 397 | class TestNav2Lifecycle: 398 | """Tests for nav2_lifecycle tool.""" 399 | 400 | async def test_nav2_lifecycle_startup( 401 | self, 402 | test_server: FastMCP, 403 | mock_navigation_manager: Mock, 404 | lifecycle_nodes: List[str] 405 | ) -> None: 406 | """Test successful Nav2 lifecycle startup.""" 407 | with patch( 408 | 'nav2_mcp_server.tools.get_navigation_manager', 409 | return_value=mock_navigation_manager 410 | ): 411 | with patch('nav2_mcp_server.tools.get_transform_manager'): 412 | async with Client(test_server) as client: 413 | result = await client.call_tool( 414 | 'nav2_lifecycle', 415 | {'operation': 'startup'} 416 | ) 417 | 418 | assert result.content 419 | mock_navigation_manager.lifecycle_startup.assert_called_once() 420 | 421 | async def test_nav2_lifecycle_shutdown( 422 | self, 423 | test_server: FastMCP, 424 | mock_navigation_manager: Mock 425 | ) -> None: 426 | """Test successful Nav2 lifecycle shutdown.""" 427 | with patch( 428 | 'nav2_mcp_server.tools.get_navigation_manager', 429 | return_value=mock_navigation_manager 430 | ): 431 | with patch('nav2_mcp_server.tools.get_transform_manager'): 432 | async with Client(test_server) as client: 433 | result = await client.call_tool( 434 | 'nav2_lifecycle', 435 | {'operation': 'shutdown'} 436 | ) 437 | 438 | assert result.content 439 | mock_navigation_manager.lifecycle_shutdown.assert_called_once() 440 | 441 | async def test_nav2_lifecycle_invalid_operation( 442 | self, 443 | test_server: FastMCP, 444 | mock_navigation_manager: Mock 445 | ) -> None: 446 | """Test Nav2 lifecycle with invalid operation. 447 | 448 | Verifies that an invalid operation parameter results in an error. 449 | """ 450 | with patch( 451 | 'nav2_mcp_server.tools.get_navigation_manager', 452 | return_value=mock_navigation_manager 453 | ): 454 | with patch('nav2_mcp_server.tools.get_transform_manager'): 455 | async with Client(test_server) as client: 456 | # Should handle invalid operation gracefully 457 | result = await client.call_tool( 458 | 'nav2_lifecycle', 459 | {'operation': 'invalid_operation'} 460 | ) 461 | 462 | # Result should contain error information 463 | assert result.content 464 | # The decorator should catch the ValueError and return JSON 465 | 466 | 467 | class TestToolErrorHandling: 468 | """Tests for error handling across all tools.""" 469 | 470 | async def test_navigation_manager_exception( 471 | self, 472 | test_server: FastMCP 473 | ) -> None: 474 | """Test tool behavior when NavigationManager raises exception.""" 475 | mock_nav_manager = Mock() 476 | mock_nav_manager.navigate_to_pose.side_effect = Exception( 477 | 'Navigation system unavailable' 478 | ) 479 | 480 | with patch( 481 | 'nav2_mcp_server.tools.get_navigation_manager', 482 | return_value=mock_nav_manager 483 | ): 484 | with patch('nav2_mcp_server.tools.get_transform_manager'): 485 | async with Client(test_server) as client: 486 | # Should handle the exception and return error info 487 | result = await client.call_tool( 488 | 'navigate_to_pose', 489 | { 490 | 'x': 1.0, 491 | 'y': 2.0, 492 | 'yaw': 1.57 493 | } 494 | ) 495 | 496 | # Result should contain error information 497 | assert result.content 498 | assert 'error' in str(result.content).lower() 499 | 500 | async def test_transform_manager_exception( 501 | self, 502 | test_server: FastMCP 503 | ) -> None: 504 | """Test tool behavior when TransformManager raises exception.""" 505 | mock_tf_manager = Mock() 506 | mock_tf_manager.get_robot_pose.side_effect = Exception( 507 | 'Transform lookup failed' 508 | ) 509 | 510 | with patch('nav2_mcp_server.tools.get_navigation_manager'): 511 | with patch( 512 | 'nav2_mcp_server.tools.get_transform_manager', 513 | return_value=mock_tf_manager 514 | ): 515 | async with Client(test_server) as client: 516 | # Should handle the exception and return error info 517 | result = await client.call_tool( 518 | 'get_robot_pose', 519 | {} 520 | ) 521 | 522 | # Result should contain error information 523 | assert result.content 524 | assert 'error' in str(result.content).lower() 525 | -------------------------------------------------------------------------------- /tests/test_transforms.py: -------------------------------------------------------------------------------- 1 | """Tests for TransformManager operations. 2 | 3 | This module tests the TransformManager class and its transform 4 | operations including robot pose retrieval and coordinate transformations. 5 | """ 6 | 7 | import math 8 | from unittest.mock import Mock, patch 9 | 10 | import pytest 11 | 12 | from nav2_mcp_server.transforms import TransformManager 13 | from nav2_mcp_server.utils import MCPContextManager 14 | 15 | 16 | class TestTransformManagerInitialization: 17 | """Tests for TransformManager initialization.""" 18 | 19 | def test_transform_manager_init(self) -> None: 20 | """Test TransformManager initialization. 21 | 22 | Verifies that the TransformManager properly initializes. 23 | """ 24 | tf_manager = TransformManager() 25 | 26 | assert tf_manager is not None 27 | assert tf_manager._node is None # Node not created until needed 28 | 29 | @patch('nav2_mcp_server.transforms.rclpy.spin_once') 30 | @patch('nav2_mcp_server.transforms.TransformListener') 31 | @patch('nav2_mcp_server.transforms.Buffer') 32 | @patch('nav2_mcp_server.transforms.Node') 33 | @patch('nav2_mcp_server.transforms.rclpy.init') 34 | def test_transform_manager_tf_setup( 35 | self, 36 | mock_init: Mock, 37 | mock_node_class: Mock, 38 | mock_buffer_class: Mock, 39 | mock_listener_class: Mock, 40 | mock_spin: Mock 41 | ) -> None: 42 | """Test TransformManager TF setup. 43 | 44 | Verifies that TF2 buffer and listener are properly configured. 45 | """ 46 | mock_node = Mock() 47 | mock_node_class.return_value = mock_node 48 | mock_buffer_instance = Mock() 49 | mock_buffer_class.return_value = mock_buffer_instance 50 | 51 | # Initialize rclpy for the test 52 | mock_init.return_value = None 53 | 54 | tf_manager = TransformManager() 55 | tf_manager._ensure_tf_setup() 56 | 57 | mock_node_class.assert_called_once() 58 | mock_buffer_class.assert_called_once() 59 | mock_listener_class.assert_called_once() 60 | 61 | 62 | class TestGetRobotPose: 63 | """Tests for robot pose retrieval functionality.""" 64 | 65 | @patch('nav2_mcp_server.transforms.rclpy.spin_once') 66 | @patch('nav2_mcp_server.transforms.TransformListener') 67 | @patch('nav2_mcp_server.transforms.Buffer') 68 | @patch('nav2_mcp_server.transforms.Node') 69 | def test_get_robot_pose_success( 70 | self, 71 | mock_node_class: Mock, 72 | mock_buffer_class: Mock, 73 | mock_listener_class: Mock, 74 | mock_spin: Mock 75 | ) -> None: 76 | """Test successful robot pose retrieval. 77 | 78 | Verifies that robot pose is correctly retrieved from TF2. 79 | """ 80 | # Setup mocks 81 | mock_node = Mock() 82 | mock_node_class.return_value = mock_node 83 | mock_buffer_instance = Mock() 84 | mock_buffer_class.return_value = mock_buffer_instance 85 | 86 | # Mock transform 87 | mock_transform = Mock() 88 | mock_transform.transform.translation.x = 1.0 89 | mock_transform.transform.translation.y = 2.0 90 | mock_transform.transform.translation.z = 0.0 91 | mock_transform.transform.rotation.x = 0.0 92 | mock_transform.transform.rotation.y = 0.0 93 | mock_transform.transform.rotation.z = 0.0 94 | mock_transform.transform.rotation.w = 1.0 95 | mock_transform.header.stamp.sec = 100 96 | mock_transform.header.stamp.nanosec = 0 97 | 98 | mock_buffer_instance.can_transform.return_value = True 99 | mock_buffer_instance.lookup_transform.return_value = mock_transform 100 | 101 | tf_manager = TransformManager() 102 | context_manager = MCPContextManager() 103 | 104 | result = tf_manager.get_robot_pose(context_manager) 105 | 106 | assert 'position' in result 107 | assert 'orientation' in result 108 | assert result['position']['x'] == 1.0 109 | assert result['position']['y'] == 2.0 110 | 111 | @patch('nav2_mcp_server.transforms.rclpy.spin_once') 112 | @patch('nav2_mcp_server.transforms.TransformListener') 113 | @patch('nav2_mcp_server.transforms.Buffer') 114 | @patch('nav2_mcp_server.transforms.Node') 115 | def test_get_robot_pose_transform_failure( 116 | self, 117 | mock_node_class: Mock, 118 | mock_buffer_class: Mock, 119 | mock_listener_class: Mock, 120 | mock_spin: Mock 121 | ) -> None: 122 | """Test robot pose retrieval with transform failure. 123 | 124 | Verifies that transform lookup failures are handled gracefully. 125 | """ 126 | from nav2_mcp_server.exceptions import TransformError 127 | 128 | # Setup mocks 129 | mock_node = Mock() 130 | mock_node_class.return_value = mock_node 131 | mock_buffer_instance = Mock() 132 | mock_buffer_class.return_value = mock_buffer_instance 133 | 134 | # Mock transform failure 135 | mock_buffer_instance.can_transform.return_value = True 136 | mock_buffer_instance.lookup_transform.side_effect = Exception( 137 | 'Transform not available' 138 | ) 139 | 140 | tf_manager = TransformManager() 141 | context_manager = MCPContextManager() 142 | 143 | # Should raise TransformError 144 | with pytest.raises(TransformError): 145 | tf_manager.get_robot_pose(context_manager) 146 | 147 | @patch('nav2_mcp_server.transforms.rclpy.spin_once') 148 | @patch('nav2_mcp_server.transforms.TransformListener') 149 | @patch('nav2_mcp_server.transforms.Buffer') 150 | @patch('nav2_mcp_server.transforms.Node') 151 | def test_get_robot_pose_with_custom_frames( 152 | self, 153 | mock_node_class: Mock, 154 | mock_buffer_class: Mock, 155 | mock_listener_class: Mock, 156 | mock_spin: Mock 157 | ) -> None: 158 | """Test robot pose retrieval works with config frames. 159 | 160 | Verifies that the method uses frames from config. 161 | """ 162 | # Setup mocks 163 | mock_node = Mock() 164 | mock_node_class.return_value = mock_node 165 | mock_buffer_instance = Mock() 166 | mock_buffer_class.return_value = mock_buffer_instance 167 | 168 | # Mock transform 169 | mock_transform = Mock() 170 | mock_transform.transform.translation.x = 3.0 171 | mock_transform.transform.translation.y = 4.0 172 | mock_transform.transform.translation.z = 0.5 173 | mock_transform.transform.rotation.x = 0.0 174 | mock_transform.transform.rotation.y = 0.0 175 | mock_transform.transform.rotation.z = 0.707 176 | mock_transform.transform.rotation.w = 0.707 177 | mock_transform.header.stamp.sec = 100 178 | mock_transform.header.stamp.nanosec = 0 179 | 180 | mock_buffer_instance.can_transform.return_value = True 181 | mock_buffer_instance.lookup_transform.return_value = mock_transform 182 | 183 | tf_manager = TransformManager() 184 | context_manager = MCPContextManager() 185 | 186 | # Note: get_robot_pose doesn't take frame arguments, uses config 187 | result = tf_manager.get_robot_pose(context_manager) 188 | 189 | assert result['position']['x'] == 3.0 190 | assert result['position']['y'] == 4.0 191 | assert result['position']['z'] == 0.5 192 | 193 | 194 | class TestPoseExtraction: 195 | """Tests for pose extraction from transforms.""" 196 | 197 | def test_extract_pose_from_transform(self) -> None: 198 | """Test pose extraction from transform message. 199 | 200 | Verifies that transform messages are correctly converted 201 | to pose dictionaries. 202 | """ 203 | tf_manager = TransformManager() 204 | 205 | # Create mock transform 206 | mock_transform = Mock() 207 | mock_transform.transform.translation.x = 1.5 208 | mock_transform.transform.translation.y = -2.3 209 | mock_transform.transform.translation.z = 0.1 210 | mock_transform.transform.rotation.x = 0.0 211 | mock_transform.transform.rotation.y = 0.0 212 | mock_transform.transform.rotation.z = 0.383 213 | mock_transform.transform.rotation.w = 0.924 214 | mock_transform.header.stamp.sec = 100 215 | mock_transform.header.stamp.nanosec = 0 216 | 217 | result = tf_manager._extract_pose_from_transform(mock_transform) 218 | 219 | assert 'position' in result 220 | assert 'orientation' in result 221 | assert result['position']['x'] == 1.5 222 | assert result['position']['y'] == -2.3 223 | assert result['position']['z'] == 0.1 224 | # Check quaternion inside orientation 225 | assert result['orientation']['quaternion']['z'] == 0.383 226 | assert result['orientation']['quaternion']['w'] == 0.924 227 | 228 | 229 | class TestQuaternionOperations: 230 | """Tests for quaternion operations.""" 231 | 232 | def test_quaternion_to_yaw_conversion(self) -> None: 233 | """Test quaternion to yaw angle conversion. 234 | 235 | Verifies that quaternions are correctly converted to yaw angles. 236 | """ 237 | tf_manager = TransformManager() 238 | 239 | # Test known quaternion conversions 240 | # Identity quaternion (0 degrees) - w=1, others=0 241 | yaw_0 = tf_manager._quaternion_to_yaw(1.0, 0.0, 0.0, 0.0) 242 | assert abs(yaw_0) < 0.001 243 | 244 | # 90 degrees rotation 245 | yaw_90 = tf_manager._quaternion_to_yaw(0.707, 0.0, 0.0, 0.707) 246 | assert abs(yaw_90 - math.pi / 2) < 0.01 247 | 248 | # 180 degrees rotation 249 | yaw_180 = tf_manager._quaternion_to_yaw(0.0, 0.0, 0.0, 1.0) 250 | assert abs(abs(yaw_180) - math.pi) < 0.01 251 | 252 | def test_yaw_to_quaternion_conversion(self) -> None: 253 | """Test yaw angle to quaternion conversion. 254 | 255 | Verifies that yaw angles are correctly converted to quaternions. 256 | """ 257 | tf_manager = TransformManager() 258 | 259 | # Test known angle conversions 260 | # 0 degrees 261 | quat_0 = tf_manager.yaw_to_quaternion(0.0) 262 | assert abs(quat_0['w'] - 1.0) < 0.001 263 | assert abs(quat_0['z']) < 0.001 264 | 265 | # 90 degrees 266 | quat_90 = tf_manager.yaw_to_quaternion(math.pi / 2) 267 | assert abs(quat_90['z'] - 0.707) < 0.01 268 | assert abs(quat_90['w'] - 0.707) < 0.01 269 | 270 | # 180 degrees 271 | quat_180 = tf_manager.yaw_to_quaternion(math.pi) 272 | assert abs(quat_180['w']) < 0.01 273 | assert abs(abs(quat_180['z']) - 1.0) < 0.01 274 | 275 | def test_yaw_quaternion_roundtrip(self) -> None: 276 | """Test yaw-quaternion-yaw conversion roundtrip. 277 | 278 | Verifies that converting yaw to quaternion and back 279 | preserves the original value. 280 | """ 281 | tf_manager = TransformManager() 282 | 283 | test_angles = [0.0, math.pi / 4, math.pi / 2, math.pi, -math.pi / 2] 284 | 285 | for original_yaw in test_angles: 286 | # Convert to quaternion and back 287 | quat = tf_manager.yaw_to_quaternion(original_yaw) 288 | # Note: _quaternion_to_yaw signature is (w, x, y, z) 289 | recovered_yaw = tf_manager._quaternion_to_yaw( 290 | quat['w'], quat['x'], quat['y'], quat['z'] 291 | ) 292 | 293 | # Account for angle wrapping 294 | diff = abs(original_yaw - recovered_yaw) 295 | if diff > math.pi: 296 | diff = 2 * math.pi - diff 297 | 298 | assert diff < 0.01, f'Roundtrip failed for {original_yaw}' 299 | 300 | 301 | class TestTransformManagerDestroy: 302 | """Tests for TransformManager cleanup.""" 303 | 304 | @patch('nav2_mcp_server.transforms.Node') 305 | def test_transform_manager_destroy(self, mock_node_class: Mock) -> None: 306 | """Test TransformManager cleanup. 307 | 308 | Verifies that the TransformManager properly cleans up 309 | ROS2 resources on destruction. 310 | """ 311 | mock_node = Mock() 312 | mock_node_class.return_value = mock_node 313 | 314 | tf_manager = TransformManager() 315 | # Setup the node first 316 | tf_manager._node = mock_node 317 | 318 | # Now destroy 319 | tf_manager.destroy() 320 | 321 | # Verify node cleanup is called 322 | mock_node.destroy_node.assert_called_once() 323 | 324 | @patch('nav2_mcp_server.transforms.Node') 325 | def test_transform_manager_destroy_with_tf_listener( 326 | self, mock_node_class: Mock 327 | ) -> None: 328 | """Test TransformManager cleanup with TF listener. 329 | 330 | Verifies that the TF listener is properly cleaned up. 331 | """ 332 | mock_node = Mock() 333 | mock_node_class.return_value = mock_node 334 | mock_tf_listener = Mock() 335 | mock_tf_buffer = Mock() 336 | 337 | tf_manager = TransformManager() 338 | tf_manager._node = mock_node 339 | tf_manager._tf_listener = mock_tf_listener 340 | tf_manager._tf_buffer = mock_tf_buffer 341 | 342 | # Destroy 343 | tf_manager.destroy() 344 | 345 | # Verify cleanup 346 | assert tf_manager._tf_listener is None 347 | assert tf_manager._tf_buffer is None 348 | assert tf_manager._node is None 349 | mock_node.destroy_node.assert_called_once() 350 | 351 | 352 | class TestTransformManagerGlobal: 353 | """Tests for global transform manager instance.""" 354 | 355 | def test_get_transform_manager_creates_instance(self) -> None: 356 | """Test that get_transform_manager creates a new instance. 357 | 358 | Verifies that the global function creates a manager when needed. 359 | """ 360 | from nav2_mcp_server import transforms 361 | 362 | # Reset global instance 363 | transforms._transform_manager = None 364 | 365 | # Get manager 366 | manager = transforms.get_transform_manager() 367 | 368 | assert manager is not None 369 | assert isinstance(manager, TransformManager) 370 | 371 | def test_get_transform_manager_returns_same_instance(self) -> None: 372 | """Test that get_transform_manager returns the same instance. 373 | 374 | Verifies that the global function returns a singleton instance. 375 | """ 376 | from nav2_mcp_server import transforms 377 | 378 | # Reset global instance 379 | transforms._transform_manager = None 380 | 381 | # Get manager twice 382 | manager1 = transforms.get_transform_manager() 383 | manager2 = transforms.get_transform_manager() 384 | 385 | assert manager1 is manager2 386 | 387 | 388 | class TestTransformManagerErrorHandling: 389 | """Tests for TransformManager error handling.""" 390 | 391 | @patch('nav2_mcp_server.transforms.rclpy.spin_once') 392 | @patch('nav2_mcp_server.transforms.TransformListener') 393 | @patch('nav2_mcp_server.transforms.Buffer') 394 | @patch('nav2_mcp_server.transforms.Node') 395 | def test_get_robot_pose_timeout_error( 396 | self, 397 | mock_node_class: Mock, 398 | mock_buffer_class: Mock, 399 | mock_listener_class: Mock, 400 | mock_spin: Mock 401 | ) -> None: 402 | """Test robot pose retrieval with timeout error. 403 | 404 | Verifies that timeout errors are handled appropriately. 405 | """ 406 | from tf2_ros import LookupException 407 | 408 | from nav2_mcp_server.exceptions import TransformError 409 | 410 | mock_node = Mock() 411 | mock_node_class.return_value = mock_node 412 | mock_buffer_instance = Mock() 413 | mock_buffer_class.return_value = mock_buffer_instance 414 | 415 | # Mock timeout exception 416 | mock_buffer_instance.can_transform.return_value = True 417 | mock_buffer_instance.lookup_transform.side_effect = LookupException( 418 | 'Transform timeout' 419 | ) 420 | 421 | tf_manager = TransformManager() 422 | context_manager = MCPContextManager() 423 | 424 | # Should raise TransformError instead of returning error dict 425 | with pytest.raises(TransformError): 426 | tf_manager.get_robot_pose(context_manager) 427 | 428 | def test_transform_manager_init_failure(self) -> None: 429 | """Test TransformManager initialization. 430 | 431 | Verifies that TransformManager uses lazy initialization 432 | and can be created without immediate ROS2 setup. 433 | """ 434 | # Should not raise - node is created lazily when needed 435 | tf_manager = TransformManager() 436 | assert tf_manager is not None 437 | assert tf_manager._node is None # Not initialized yet 438 | 439 | @patch('nav2_mcp_server.transforms.rclpy.spin_once') 440 | @patch('nav2_mcp_server.transforms.TransformListener') 441 | @patch('nav2_mcp_server.transforms.Buffer') 442 | @patch('nav2_mcp_server.transforms.Node') 443 | def test_get_robot_pose_buffer_not_initialized( 444 | self, 445 | mock_node_class: Mock, 446 | mock_buffer_class: Mock, 447 | mock_listener_class: Mock, 448 | mock_spin: Mock 449 | ) -> None: 450 | """Test robot pose retrieval when buffer is not initialized. 451 | 452 | Verifies error handling when TF buffer is unexpectedly None. 453 | """ 454 | from nav2_mcp_server.exceptions import TransformError 455 | 456 | mock_node = Mock() 457 | mock_node_class.return_value = mock_node 458 | 459 | tf_manager = TransformManager() 460 | # Force tf_buffer to be None after setup 461 | tf_manager._ensure_tf_setup() 462 | tf_manager._tf_buffer = None 463 | 464 | context_manager = MCPContextManager() 465 | 466 | with pytest.raises(TransformError, match='TF buffer not initialized'): 467 | tf_manager.get_robot_pose(context_manager) 468 | 469 | @patch('nav2_mcp_server.transforms.rclpy.spin_once') 470 | @patch('nav2_mcp_server.transforms.TransformListener') 471 | @patch('nav2_mcp_server.transforms.Buffer') 472 | @patch('nav2_mcp_server.transforms.Node') 473 | def test_get_robot_pose_transform_timeout( 474 | self, 475 | mock_node_class: Mock, 476 | mock_buffer_class: Mock, 477 | mock_listener_class: Mock, 478 | mock_spin: Mock 479 | ) -> None: 480 | """Test robot pose when transform never becomes available. 481 | 482 | Verifies timeout handling when transform is not available. 483 | """ 484 | from typing import Any 485 | 486 | from nav2_mcp_server.exceptions import TransformError 487 | 488 | mock_node = Mock() 489 | mock_node_class.return_value = mock_node 490 | mock_buffer_instance = Mock() 491 | mock_buffer_class.return_value = mock_buffer_instance 492 | 493 | # Mock transform never available 494 | mock_buffer_instance.can_transform.return_value = False 495 | 496 | tf_manager = TransformManager() 497 | context_manager = MCPContextManager() 498 | 499 | # Limit spin_once calls to avoid infinite loop 500 | call_count = [0] 501 | 502 | def spin_side_effect(*args: Any, **kwargs: Any) -> None: 503 | call_count[0] += 1 504 | if call_count[0] > 5: 505 | # Force exception after a few tries 506 | raise TransformError( 507 | 'Could not get transform after waiting', 508 | 0, 509 | {} 510 | ) 511 | 512 | mock_spin.side_effect = spin_side_effect 513 | 514 | with pytest.raises(TransformError): 515 | tf_manager.get_robot_pose(context_manager) 516 | -------------------------------------------------------------------------------- /tests/test_utils.py: -------------------------------------------------------------------------------- 1 | """Tests for utility functions and classes. 2 | 3 | This module tests utility functions including MCPContextManager, 4 | logging setup, JSON serialization, and helper functions. 5 | """ 6 | 7 | import json 8 | from unittest.mock import AsyncMock, Mock, patch 9 | 10 | import pytest 11 | 12 | from nav2_mcp_server.utils import ( 13 | MCPContextManager, 14 | safe_json_dumps, 15 | setup_logging, 16 | with_context_logging, 17 | ) 18 | 19 | 20 | class TestMCPContextManager: 21 | """Tests for MCPContextManager class.""" 22 | 23 | def test_mcp_context_manager_init(self) -> None: 24 | """Test MCPContextManager initialization. 25 | 26 | Verifies that MCPContextManager initializes properly 27 | with default values. 28 | """ 29 | context_manager = MCPContextManager() 30 | 31 | assert context_manager is not None 32 | assert hasattr(context_manager, 'info') 33 | assert hasattr(context_manager, 'warning') 34 | assert hasattr(context_manager, 'error') 35 | 36 | def test_mcp_context_manager_info(self) -> None: 37 | """Test MCPContextManager info method. 38 | 39 | Verifies that info messages are handled correctly. 40 | """ 41 | context_manager = MCPContextManager() 42 | 43 | # Should not raise exception 44 | context_manager.info_sync('Test info message') 45 | 46 | def test_mcp_context_manager_warning(self) -> None: 47 | """Test MCPContextManager warning method. 48 | 49 | Verifies that warning messages are handled correctly. 50 | """ 51 | context_manager = MCPContextManager() 52 | 53 | # Should not raise exception 54 | context_manager.warning_sync('Test warning message') 55 | 56 | def test_mcp_context_manager_error(self) -> None: 57 | """Test MCPContextManager error method. 58 | 59 | Verifies that error messages are handled correctly. 60 | """ 61 | context_manager = MCPContextManager() 62 | 63 | # Should not raise exception when using sync methods 64 | context_manager.error_sync('Test error message') 65 | 66 | def test_mcp_context_manager_with_real_context(self) -> None: 67 | """Test MCPContextManager with real MCP Context. 68 | 69 | Verifies that MCPContextManager works with actual Context objects using sync methods. 70 | """ 71 | # Mock a real MCP Context 72 | mock_context = Mock() 73 | mock_context.info = Mock() 74 | mock_context.warning = Mock() 75 | mock_context.error = Mock() 76 | 77 | context_manager = MCPContextManager(mock_context) 78 | 79 | # Use sync methods which call anyio.from_thread.run 80 | with patch('anyio.from_thread.run'): 81 | context_manager.info_sync('Test message') 82 | context_manager.warning_sync('Warning message') 83 | context_manager.error_sync('Error message') 84 | 85 | # The logger should have been called 86 | assert context_manager.logger is not None 87 | 88 | 89 | class TestLoggingSetup: 90 | """Tests for logging setup functionality.""" 91 | 92 | @patch('nav2_mcp_server.utils.logging.basicConfig') 93 | @patch('nav2_mcp_server.utils.get_config') 94 | def test_setup_logging_default(self, mock_get_config: Mock, mock_basic_config: Mock) -> None: 95 | """Test default logging setup. 96 | 97 | Verifies that logging is configured with default settings. 98 | """ 99 | # Mock config 100 | mock_config = Mock() 101 | mock_config.logging.node_name = 'nav2_mcp_server' 102 | mock_config.logging.log_level = 'INFO' 103 | mock_config.logging.log_format = '%(asctime)s - %(levelname)s - %(message)s' 104 | mock_get_config.return_value = mock_config 105 | 106 | logger = setup_logging() 107 | 108 | assert logger is not None 109 | mock_basic_config.assert_called_once() 110 | 111 | @patch('nav2_mcp_server.utils.logging.basicConfig') 112 | @patch('nav2_mcp_server.utils.get_config') 113 | def test_setup_logging_debug_level( 114 | self, mock_get_config: Mock, mock_basic_config: Mock 115 | ) -> None: 116 | """Test logging setup with debug level. 117 | 118 | Verifies that debug logging level is correctly configured. 119 | """ 120 | # Mock config with debug level 121 | mock_config = Mock() 122 | mock_config.logging.node_name = 'nav2_mcp_server' 123 | mock_config.logging.log_level = 'DEBUG' 124 | mock_config.logging.log_format = '%(asctime)s - %(levelname)s - %(message)s' 125 | mock_get_config.return_value = mock_config 126 | 127 | logger = setup_logging() 128 | 129 | assert logger is not None 130 | mock_basic_config.assert_called_once() 131 | 132 | @patch('nav2_mcp_server.utils.logging.getLogger') 133 | @patch('nav2_mcp_server.utils.get_config') 134 | def test_setup_logging_returns_logger( 135 | self, mock_get_config: Mock, mock_get_logger: Mock 136 | ) -> None: 137 | """Test that setup_logging returns a logger instance. 138 | 139 | Verifies that the function returns a proper logger object. 140 | """ 141 | mock_config = Mock() 142 | mock_config.logging.node_name = 'nav2_mcp_server' 143 | mock_config.logging.log_level = 'INFO' 144 | mock_config.logging.log_format = '%(asctime)s - %(levelname)s - %(message)s' 145 | mock_get_config.return_value = mock_config 146 | 147 | mock_logger = Mock() 148 | mock_get_logger.return_value = mock_logger 149 | 150 | result = setup_logging() 151 | 152 | assert result is mock_logger 153 | mock_get_logger.assert_called_once_with('nav2_mcp_server') 154 | 155 | 156 | class TestSafeJSONDumps: 157 | """Tests for safe JSON serialization.""" 158 | 159 | def test_safe_json_dumps_simple_dict(self) -> None: 160 | """Test JSON serialization of simple dictionary. 161 | 162 | Verifies that simple dictionaries are correctly serialized. 163 | """ 164 | test_data = { 165 | 'name': 'test', 166 | 'value': 42, 167 | 'active': True 168 | } 169 | 170 | result = safe_json_dumps(test_data) 171 | 172 | assert isinstance(result, str) 173 | parsed = json.loads(result) 174 | assert parsed == test_data 175 | 176 | def test_safe_json_dumps_nested_dict(self) -> None: 177 | """Test JSON serialization of nested dictionary. 178 | 179 | Verifies that nested data structures are correctly serialized. 180 | """ 181 | test_data = { 182 | 'robot': { 183 | 'position': {'x': 1.0, 'y': 2.0, 'z': 0.0}, 184 | 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.707, 'w': 0.707} 185 | }, 186 | 'status': 'active', 187 | 'sensors': ['lidar', 'camera', 'imu'] 188 | } 189 | 190 | result = safe_json_dumps(test_data) 191 | 192 | assert isinstance(result, str) 193 | parsed = json.loads(result) 194 | assert parsed == test_data 195 | 196 | def test_safe_json_dumps_with_none_values(self) -> None: 197 | """Test JSON serialization with None values. 198 | 199 | Verifies that None values are handled correctly. 200 | """ 201 | test_data = { 202 | 'value': None, 203 | 'other': 'test' 204 | } 205 | 206 | result = safe_json_dumps(test_data) 207 | 208 | assert isinstance(result, str) 209 | parsed = json.loads(result) 210 | assert parsed == test_data 211 | 212 | def test_safe_json_dumps_error_handling(self) -> None: 213 | """Test JSON serialization error handling. 214 | 215 | Verifies that non-serializable objects are handled gracefully. 216 | """ 217 | # Create a non-serializable object 218 | class NonSerializable: 219 | pass 220 | 221 | test_data = { 222 | 'good': 'value', 223 | 'bad': NonSerializable() 224 | } 225 | 226 | result = safe_json_dumps(test_data) 227 | 228 | # Should return a JSON string even with error 229 | assert isinstance(result, str) 230 | parsed = json.loads(result) 231 | assert 'error' in parsed or 'good' in parsed 232 | 233 | def test_safe_json_dumps_with_formatting(self) -> None: 234 | """Test JSON serialization with formatting options. 235 | 236 | Verifies that JSON formatting options work correctly. 237 | """ 238 | test_data = {'a': 1, 'b': 2} 239 | 240 | result = safe_json_dumps(test_data, indent=2) 241 | 242 | assert isinstance(result, str) 243 | assert '\n' in result # Should have newlines due to indent 244 | parsed = json.loads(result) 245 | assert parsed == test_data 246 | 247 | 248 | class TestWithContextLogging: 249 | """Tests for context logging decorator.""" 250 | 251 | def test_with_context_logging_decorator(self) -> None: 252 | """Test context logging decorator functionality. 253 | 254 | Verifies that the decorator properly wraps functions 255 | and adds context logging. 256 | """ 257 | # Mock function to decorate 258 | @with_context_logging 259 | def test_function(x: int, y: int, context_manager: MCPContextManager | None = None) -> str: 260 | return f'success: {x} + {y}' 261 | 262 | # The decorator extracts ctx from kwargs and creates context_manager 263 | result = test_function(1, 2, context_manager=None) 264 | 265 | assert 'success' in result 266 | 267 | def test_with_context_logging_with_exception(self) -> None: 268 | """Test context logging decorator with exception. 269 | 270 | Verifies that exceptions are properly caught and returned as JSON. 271 | """ 272 | from nav2_mcp_server.exceptions import NavigationError, NavigationErrorCode 273 | 274 | @with_context_logging 275 | def failing_function(context_manager: MCPContextManager | None = None) -> str: 276 | raise NavigationError( 277 | 'Test error', 278 | NavigationErrorCode.NAVIGATION_FAILED, 279 | {} 280 | ) 281 | 282 | # Should return JSON error instead of raising 283 | result = failing_function(context_manager=None) 284 | assert isinstance(result, str) 285 | parsed = json.loads(result) 286 | assert 'error' in parsed or 'message' in parsed 287 | 288 | def test_with_context_logging_generic_exception(self) -> None: 289 | """Test context logging decorator with generic exception. 290 | 291 | Verifies that generic exceptions are properly caught and returned as JSON. 292 | """ 293 | @with_context_logging 294 | def failing_function(context_manager: MCPContextManager | None = None) -> str: 295 | raise ValueError('Test error') 296 | 297 | # Should return JSON error instead of raising 298 | result = failing_function(context_manager=None) 299 | assert isinstance(result, str) 300 | parsed = json.loads(result) 301 | assert 'error' in parsed or 'message' in parsed 302 | 303 | 304 | class TestUtilityFunctions: 305 | """Tests for miscellaneous utility functions.""" 306 | 307 | def test_utility_functions_exist(self) -> None: 308 | """Test that expected utility functions exist. 309 | 310 | Verifies that the module provides expected utility functions. 311 | """ 312 | from nav2_mcp_server import utils 313 | 314 | # Check that expected functions/classes exist 315 | assert hasattr(utils, 'MCPContextManager') 316 | assert hasattr(utils, 'setup_logging') 317 | assert hasattr(utils, 'safe_json_dumps') 318 | assert hasattr(utils, 'with_context_logging') 319 | 320 | def test_import_structure(self) -> None: 321 | """Test import structure of utils module. 322 | 323 | Verifies that the utils module can be imported correctly. 324 | """ 325 | # Should be able to import the module 326 | import nav2_mcp_server.utils 327 | 328 | # Module should exist 329 | assert nav2_mcp_server.utils is not None 330 | 331 | 332 | class TestErrorHandling: 333 | """Tests for error handling in utility functions.""" 334 | 335 | def test_mcp_context_manager_with_broken_context(self) -> None: 336 | """Test MCPContextManager with broken context object. 337 | 338 | Verifies that broken context objects cause expected exceptions. 339 | """ 340 | # Create a mock context that raises exceptions 341 | broken_context = Mock() 342 | broken_context.info.side_effect = Exception('Context broken') 343 | 344 | context_manager = MCPContextManager(broken_context) 345 | 346 | # Should raise exception when context is broken 347 | with pytest.raises(Exception): 348 | context_manager.info_sync('Test message') 349 | 350 | def test_safe_json_dumps_circular_reference(self) -> None: 351 | """Test JSON serialization with circular reference. 352 | 353 | Verifies that circular references are handled gracefully. 354 | """ 355 | # Create circular reference 356 | test_data: dict[str, object] = {} 357 | test_data['self'] = test_data 358 | 359 | result = safe_json_dumps(test_data) 360 | 361 | # Should return some JSON string, even if it's an error message 362 | assert isinstance(result, str) 363 | # Should be valid JSON 364 | parsed = json.loads(result) 365 | assert isinstance(parsed, dict) 366 | 367 | 368 | class TestAsyncContextManagerMethods: 369 | """Tests for async methods of MCPContextManager.""" 370 | 371 | async def test_async_info_with_context(self) -> None: 372 | """Test async info method with context. 373 | 374 | Verifies that async info method works correctly with context. 375 | """ 376 | mock_context = Mock() 377 | mock_context.info = AsyncMock() 378 | 379 | context_manager = MCPContextManager(mock_context) 380 | 381 | await context_manager.info('Test async info message') 382 | 383 | # Context info should be called 384 | mock_context.info.assert_called_once_with('Test async info message') 385 | 386 | async def test_async_error_with_context(self) -> None: 387 | """Test async error method with context. 388 | 389 | Verifies that async error method works correctly with context. 390 | """ 391 | mock_context = Mock() 392 | mock_context.error = AsyncMock() 393 | 394 | context_manager = MCPContextManager(mock_context) 395 | 396 | await context_manager.error('Test async error message') 397 | 398 | # Context error should be called 399 | mock_context.error.assert_called_once_with('Test async error message') 400 | 401 | async def test_async_warning_with_context(self) -> None: 402 | """Test async warning method with context. 403 | 404 | Verifies that async warning method works correctly with context. 405 | """ 406 | mock_context = Mock() 407 | mock_context.warning = AsyncMock() 408 | 409 | context_manager = MCPContextManager(mock_context) 410 | 411 | await context_manager.warning('Test async warning message') 412 | 413 | # Context warning should be called 414 | mock_context.warning.assert_called_once_with('Test async warning message') 415 | 416 | async def test_async_info_without_context(self) -> None: 417 | """Test async info method without context. 418 | 419 | Verifies that async info method works without context. 420 | """ 421 | context_manager = MCPContextManager() 422 | 423 | # Should not raise exception 424 | await context_manager.info('Test message without context') 425 | 426 | async def test_async_error_without_context(self) -> None: 427 | """Test async error method without context. 428 | 429 | Verifies that async error method works without context. 430 | """ 431 | context_manager = MCPContextManager() 432 | 433 | # Should not raise exception 434 | await context_manager.error('Test error without context') 435 | 436 | async def test_async_warning_without_context(self) -> None: 437 | """Test async warning method without context. 438 | 439 | Verifies that async warning method works without context. 440 | """ 441 | context_manager = MCPContextManager() 442 | 443 | # Should not raise exception 444 | await context_manager.warning('Test warning without context') 445 | 446 | 447 | class TestNav2ActiveCheck: 448 | """Tests for Nav2 active check decorator.""" 449 | 450 | def test_with_nav2_active_check_decorator(self) -> None: 451 | """Test Nav2 active check decorator functionality. 452 | 453 | Verifies that the decorator properly checks Nav2 activation. 454 | """ 455 | from nav2_mcp_server.utils import with_nav2_active_check 456 | 457 | @with_nav2_active_check 458 | def test_function(x: int, y: int, context_manager: MCPContextManager | None = None) -> str: 459 | return f'Nav2 active: {x} + {y}' 460 | 461 | # Mock the navigator - patch where it's imported (inside the decorator) 462 | with patch('nav2_mcp_server.navigation.get_navigator') as mock_get_nav: 463 | mock_navigator = Mock() 464 | mock_navigator.waitUntilNav2Active = Mock() 465 | mock_get_nav.return_value = mock_navigator 466 | 467 | result = test_function(1, 2, context_manager=None) 468 | 469 | assert 'Nav2 active' in result 470 | mock_navigator.waitUntilNav2Active.assert_called_once() 471 | 472 | def test_with_nav2_active_check_with_context(self) -> None: 473 | """Test Nav2 active check with context manager. 474 | 475 | Verifies that context logging works during Nav2 activation. 476 | """ 477 | from nav2_mcp_server.utils import with_nav2_active_check 478 | 479 | @with_nav2_active_check 480 | def test_function(context_manager: MCPContextManager | None = None) -> str: 481 | return 'success' 482 | 483 | # Create a real context manager 484 | context_manager = MCPContextManager() 485 | 486 | with patch('nav2_mcp_server.navigation.get_navigator') as mock_get_nav: 487 | with patch('anyio.from_thread.run'): 488 | mock_navigator = Mock() 489 | mock_navigator.waitUntilNav2Active = Mock() 490 | mock_get_nav.return_value = mock_navigator 491 | 492 | result = test_function(context_manager=context_manager) 493 | 494 | assert result == 'success' 495 | mock_navigator.waitUntilNav2Active.assert_called_once() 496 | 497 | def test_with_nav2_active_check_failure(self) -> None: 498 | """Test Nav2 active check decorator with activation failure. 499 | 500 | Verifies that exceptions during Nav2 activation are handled. 501 | """ 502 | from nav2_mcp_server.exceptions import ROSError 503 | from nav2_mcp_server.utils import with_nav2_active_check 504 | 505 | @with_nav2_active_check 506 | def test_function(context_manager: MCPContextManager | None = None) -> str: 507 | return 'should not reach here' 508 | 509 | with patch('nav2_mcp_server.navigation.get_navigator') as mock_get_nav: 510 | mock_navigator = Mock() 511 | mock_navigator.waitUntilNav2Active.side_effect = Exception('Nav2 not available') 512 | mock_get_nav.return_value = mock_navigator 513 | 514 | with pytest.raises(ROSError, match='Failed to activate Nav2'): 515 | test_function(context_manager=None) 516 | 517 | def test_with_nav2_active_check_failure_with_context(self) -> None: 518 | """Test Nav2 active check failure with context logging. 519 | 520 | Verifies that errors are logged to context when Nav2 activation fails. 521 | """ 522 | from nav2_mcp_server.exceptions import ROSError 523 | from nav2_mcp_server.utils import with_nav2_active_check 524 | 525 | @with_nav2_active_check 526 | def test_function(context_manager: MCPContextManager | None = None) -> str: 527 | return 'should not reach here' 528 | 529 | context_manager = MCPContextManager() 530 | 531 | with patch('nav2_mcp_server.navigation.get_navigator') as mock_get_nav: 532 | with patch('anyio.from_thread.run'): 533 | mock_navigator = Mock() 534 | mock_navigator.waitUntilNav2Active.side_effect = Exception('Nav2 timeout') 535 | mock_get_nav.return_value = mock_navigator 536 | 537 | with pytest.raises(ROSError, match='Failed to activate Nav2'): 538 | test_function(context_manager=context_manager) 539 | 540 | 541 | class TestUtilityHelpers: 542 | """Tests for utility helper functions.""" 543 | 544 | def test_create_success_message(self) -> None: 545 | """Test success message creation. 546 | 547 | Verifies that success messages are formatted correctly. 548 | """ 549 | from nav2_mcp_server.utils import create_success_message 550 | 551 | result = create_success_message('navigate', {'x': 1.0, 'y': 2.0}) 552 | 553 | assert 'Successfully navigate' in result 554 | assert 'x=1.0' in result 555 | assert 'y=2.0' in result 556 | 557 | def test_create_success_message_empty_details(self) -> None: 558 | """Test success message with empty details. 559 | 560 | Verifies that empty details are handled correctly. 561 | """ 562 | from nav2_mcp_server.utils import create_success_message 563 | 564 | result = create_success_message('navigate', {}) 565 | 566 | assert 'Successfully navigate' in result 567 | 568 | def test_create_failure_message(self) -> None: 569 | """Test failure message creation. 570 | 571 | Verifies that failure messages are formatted correctly. 572 | """ 573 | from nav2_mcp_server.utils import create_failure_message 574 | 575 | result = create_failure_message( 576 | 'navigate', 577 | 'timeout', 578 | {'duration': 30} 579 | ) 580 | 581 | assert 'Failed to navigate' in result 582 | assert 'timeout' in result 583 | assert 'duration=30' in result 584 | 585 | def test_create_failure_message_no_details(self) -> None: 586 | """Test failure message without details. 587 | 588 | Verifies that failure messages work without details. 589 | """ 590 | from nav2_mcp_server.utils import create_failure_message 591 | 592 | result = create_failure_message('navigate', 'timeout') 593 | 594 | assert 'Failed to navigate' in result 595 | assert 'timeout' in result 596 | assert '(' not in result # No details parentheses 597 | 598 | def test_validate_numeric_range_valid(self) -> None: 599 | """Test numeric range validation with valid value. 600 | 601 | Verifies that valid values pass validation. 602 | """ 603 | from nav2_mcp_server.utils import validate_numeric_range 604 | 605 | # Should not raise exception 606 | validate_numeric_range(5.0, 0.0, 10.0, 'test_param') 607 | validate_numeric_range(0.0, 0.0, 10.0, 'test_param') 608 | validate_numeric_range(10.0, 0.0, 10.0, 'test_param') 609 | 610 | def test_validate_numeric_range_invalid(self) -> None: 611 | """Test numeric range validation with invalid value. 612 | 613 | Verifies that out-of-range values raise ValueError. 614 | """ 615 | from nav2_mcp_server.utils import validate_numeric_range 616 | 617 | with pytest.raises(ValueError, match='test_param must be between'): 618 | validate_numeric_range(-1.0, 0.0, 10.0, 'test_param') 619 | 620 | with pytest.raises(ValueError, match='test_param must be between'): 621 | validate_numeric_range(11.0, 0.0, 10.0, 'test_param') 622 | --------------------------------------------------------------------------------