From 204a04fb9ed7ea246adda71bd8cd811835a0d9a3 Mon Sep 17 00:00:00 2001 From: Marco Wallner Date: Tue, 4 Jul 2023 15:48:42 +0200 Subject: [PATCH] initial commit --- .github/actions/lint/action.yml | 6 + .github/actions/lint/run.sh | 6 + .github/actions/test_humble/action.yml | 6 + .github/actions/test_humble/run.sh | 12 + .github/actions/test_iron/action.yml | 6 + .github/actions/test_iron/run.sh | 12 + .github/build.sh | 11 + .github/dependabot.yml | 7 + .github/setup.sh | 25 + .github/test.sh | 6 + .github/workflows/docs.yaml | 20 + .github/workflows/linting.yaml | 24 + .github/workflows/ros_humble.yaml | 19 + .github/workflows/ros_iron.yaml | 19 + .gitignore | 346 ++++++++++++++ CONTRIBUTING.md | 18 + LICENSE | 201 ++++++++ README.md | 220 +++++++++ cfg/example.yaml | 79 ++++ cfg/test.yaml | 85 ++++ cfg/test_utm.yaml | 91 ++++ docs/geodesic.md | 7 + mkdocs.yml | 10 + package.xml | 52 +++ postgis_ros_bridge/__init__.py | 1 + postgis_ros_bridge/geodesic_transform.py | 118 +++++ postgis_ros_bridge/postgis_converter.py | 155 ++++++ .../postgis_ros_bridge_publisher_node.py | 278 +++++++++++ postgis_ros_bridge/postgresql_connection.py | 65 +++ postgis_ros_bridge/query.py | 43 ++ postgis_ros_bridge/query_result_parser.py | 441 ++++++++++++++++++ resource/postgis_ros_bridge | 0 setup.cfg | 4 + setup.py | 41 ++ test/__init__.py | 0 test/sql_data/postgis_test.sql | 40 ++ test/test_copyright.py | 30 ++ test/test_flake8.py | 30 ++ test/test_pep257.py | 28 ++ test/test_postgis_converter.py | 292 ++++++++++++ test/test_query_result_parser.py | 345 ++++++++++++++ 41 files changed, 3199 insertions(+) create mode 100644 .github/actions/lint/action.yml create mode 100755 .github/actions/lint/run.sh create mode 100644 .github/actions/test_humble/action.yml create mode 100755 .github/actions/test_humble/run.sh create mode 100644 .github/actions/test_iron/action.yml create mode 100755 .github/actions/test_iron/run.sh create mode 100755 .github/build.sh create mode 100644 .github/dependabot.yml create mode 100755 .github/setup.sh create mode 100755 .github/test.sh create mode 100644 .github/workflows/docs.yaml create mode 100644 .github/workflows/linting.yaml create mode 100644 .github/workflows/ros_humble.yaml create mode 100644 .github/workflows/ros_iron.yaml create mode 100644 .gitignore create mode 100644 CONTRIBUTING.md create mode 100644 LICENSE create mode 100644 README.md create mode 100644 cfg/example.yaml create mode 100644 cfg/test.yaml create mode 100644 cfg/test_utm.yaml create mode 100644 docs/geodesic.md create mode 100644 mkdocs.yml create mode 100644 package.xml create mode 100644 postgis_ros_bridge/__init__.py create mode 100644 postgis_ros_bridge/geodesic_transform.py create mode 100644 postgis_ros_bridge/postgis_converter.py create mode 100644 postgis_ros_bridge/postgis_ros_bridge_publisher_node.py create mode 100644 postgis_ros_bridge/postgresql_connection.py create mode 100644 postgis_ros_bridge/query.py create mode 100644 postgis_ros_bridge/query_result_parser.py create mode 100644 resource/postgis_ros_bridge create mode 100644 setup.cfg create mode 100644 setup.py create mode 100644 test/__init__.py create mode 100644 test/sql_data/postgis_test.sql create mode 100644 test/test_copyright.py create mode 100644 test/test_flake8.py create mode 100644 test/test_pep257.py create mode 100644 test/test_postgis_converter.py create mode 100644 test/test_query_result_parser.py diff --git a/.github/actions/lint/action.yml b/.github/actions/lint/action.yml new file mode 100644 index 0000000..fd0b676 --- /dev/null +++ b/.github/actions/lint/action.yml @@ -0,0 +1,6 @@ +name: 'Lint' +description: 'Lint using devcontainer' +runs: + using: 'docker' + image: 'ros:iron-ros-base-jammy' + entrypoint: ".github/actions/lint/run.sh" diff --git a/.github/actions/lint/run.sh b/.github/actions/lint/run.sh new file mode 100755 index 0000000..9090b7b --- /dev/null +++ b/.github/actions/lint/run.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +./.github/setup.sh +source /opt/ros/${ROS_DISTRO}/setup.bash +ament_${LINTER} ./ diff --git a/.github/actions/test_humble/action.yml b/.github/actions/test_humble/action.yml new file mode 100644 index 0000000..72bd7ba --- /dev/null +++ b/.github/actions/test_humble/action.yml @@ -0,0 +1,6 @@ +name: 'Test Humble' +description: 'Test using devcontainer' +runs: + using: 'docker' + image: 'ros:humble-ros-base-jammy' + entrypoint: ".github/actions/test_humble/run.sh" diff --git a/.github/actions/test_humble/run.sh b/.github/actions/test_humble/run.sh new file mode 100755 index 0000000..5cb6b01 --- /dev/null +++ b/.github/actions/test_humble/run.sh @@ -0,0 +1,12 @@ +#!/bin/bash +set -e + +./.github/setup.sh +# TODO: better method of avoiding linting / testing in build, install, log, ... +mkdir -p .build_ws/src +cp -rpa * .build_ws/src/ +cd .build_ws +mkdir -p build install log +chown postgres:postgres build install log -R +su -c ../.github/build.sh postgres +su -c ../.github/test.sh postgres diff --git a/.github/actions/test_iron/action.yml b/.github/actions/test_iron/action.yml new file mode 100644 index 0000000..8d1dec6 --- /dev/null +++ b/.github/actions/test_iron/action.yml @@ -0,0 +1,6 @@ +name: 'Test Iron' +description: 'Test using devcontainer' +runs: + using: 'docker' + image: 'ros:iron-ros-base-jammy' + entrypoint: ".github/actions/test_iron/run.sh" diff --git a/.github/actions/test_iron/run.sh b/.github/actions/test_iron/run.sh new file mode 100755 index 0000000..5cb6b01 --- /dev/null +++ b/.github/actions/test_iron/run.sh @@ -0,0 +1,12 @@ +#!/bin/bash +set -e + +./.github/setup.sh +# TODO: better method of avoiding linting / testing in build, install, log, ... +mkdir -p .build_ws/src +cp -rpa * .build_ws/src/ +cd .build_ws +mkdir -p build install log +chown postgres:postgres build install log -R +su -c ../.github/build.sh postgres +su -c ../.github/test.sh postgres diff --git a/.github/build.sh b/.github/build.sh new file mode 100755 index 0000000..27f2351 --- /dev/null +++ b/.github/build.sh @@ -0,0 +1,11 @@ +#!/bin/bash +set -e + +# Set the default build type +BUILD_TYPE=RelWithDebInfo +source /opt/ros/$ROS_DISTRO/setup.bash +colcon build \ + --merge-install \ + --symlink-install \ + --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \ + -Wall -Wextra -Wpedantic diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000..afb98ae --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,7 @@ +version: 2 +updates: +- package-ecosystem: github-actions + directory: "/" + schedule: + interval: daily + open-pull-requests-limit: 10 diff --git a/.github/setup.sh b/.github/setup.sh new file mode 100755 index 0000000..d1bea9e --- /dev/null +++ b/.github/setup.sh @@ -0,0 +1,25 @@ +#!/bin/bash +set -e + +export DEBIAN_FRONTEND=noninteractive + +apt-get update + +apt-get install -y curl + +curl https://www.postgresql.org/media/keys/ACCC4CF8.asc | gpg --dearmor | tee /etc/apt/trusted.gpg.d/apt.postgresql.org.gpg >/dev/null +echo "deb http://apt.postgresql.org/pub/repos/apt $(lsb_release -cs)-pgdg-testing main 15" | tee /etc/apt/sources.list.d/pgdg-testing.list + +apt-get update \ + && apt-get -y install --no-install-recommends \ + postgresql-14 postgresql-14-postgis-3 \ + python3-pip \ + git-lfs \ + libpq-dev \ + && apt-get autoremove -y + +python3 setup.py egg_info +pip3 install -r postgis_ros_bridge.egg-info/requires.txt + +rosdep update +rosdep install --from-paths . --ignore-src -y diff --git a/.github/test.sh b/.github/test.sh new file mode 100755 index 0000000..868acb8 --- /dev/null +++ b/.github/test.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +if [ -f install/setup.bash ]; then source install/setup.bash; fi +colcon test --merge-install --event-handlers console_cohesion+ +colcon test-result --verbose diff --git a/.github/workflows/docs.yaml b/.github/workflows/docs.yaml new file mode 100644 index 0000000..08dd53e --- /dev/null +++ b/.github/workflows/docs.yaml @@ -0,0 +1,20 @@ +name: docs + +on: + release: + push: + tags: + - 'v*' + workflow_dispatch: +permissions: + contents: write +jobs: + deploy: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v4 + with: + python-version: 3.x + - run: pip install mkdocs-simple-plugin mkdocstrings[python] + - run: mkdocs gh-deploy --force \ No newline at end of file diff --git a/.github/workflows/linting.yaml b/.github/workflows/linting.yaml new file mode 100644 index 0000000..22de6c0 --- /dev/null +++ b/.github/workflows/linting.yaml @@ -0,0 +1,24 @@ +name: Linting + +on: + pull_request: + push: + workflow_dispatch: + +jobs: + lint: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + linter: [xmllint, flake8, pep257] + steps: + - + name: Checkout code + uses: actions/checkout@v3 + - + name: Run linter + uses: ./.github/actions/lint/ + env: + LINTER: ${{ matrix.linter }} diff --git a/.github/workflows/ros_humble.yaml b/.github/workflows/ros_humble.yaml new file mode 100644 index 0000000..c9a24a5 --- /dev/null +++ b/.github/workflows/ros_humble.yaml @@ -0,0 +1,19 @@ +name: ROS Humble Hawksbill + +on: + pull_request: + push: + workflow_dispatch: + +jobs: + test_humble: + name: test_humble + runs-on: ubuntu-latest + steps: + - + name: Checkout code + uses: actions/checkout@v3 + - + name: Test Humble + uses: ./.github/actions/test_humble/ + diff --git a/.github/workflows/ros_iron.yaml b/.github/workflows/ros_iron.yaml new file mode 100644 index 0000000..4276e05 --- /dev/null +++ b/.github/workflows/ros_iron.yaml @@ -0,0 +1,19 @@ +name: ROS Iron Irwini + +on: + pull_request: + push: + workflow_dispatch: + +jobs: + test_iron: + name: test_iron + runs-on: ubuntu-latest + steps: + - + name: Checkout code + uses: actions/checkout@v3 + - + name: Test Iron + uses: ./.github/actions/test_iron/ + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0eceb4b --- /dev/null +++ b/.gitignore @@ -0,0 +1,346 @@ +.build_ws/ +test/postgres_server/*.tar + +# C +## Prerequisites +*.d + +## Object files +*.o +*.ko +*.obj +*.elf + +## Linker output +*.ilk +### (reserved) *.map +*.exp + +## Precompiled Headers +*.gch +*.pch + +## Libraries +*.lib +### (reserved) *.a +*.la +*.lo + +## Shared objects (inc. Windows DLLs) +*.dll +*.so +*.so.* +*.dylib + +## Executables +*.exe +*.out +*.app +*.i*86 +*.x86_64 +*.hex + +## Debug files +*.dSYM/ +*.su +*.idb +*.pdb + +## Kernel Module Compile Results +*.mod* +*.cmd +.tmp_versions/ +modules.order +Module.symvers +Mkfile.old +dkms.conf + + + +# C++ +## Prerequisites +*.dpp + +## Compiled Object files +*.slo + +## Fortran module files +*.smod + +## Compiled Static libraries +*.lai + + + +# Python +## Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +## Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +### (reserved) lib/ +### (reserved) lib64/ +parts/ +sdist/ +var/ +wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +## PyInstaller +## Usually these files are written by a python script from a template +## before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +## Installer logs +pip-log.txt +pip-delete-this-directory.txt + +## Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +.hypothesis/ +.pytest_cache/ + +## Translations +*.mo +*.pot + +## Django stuff: +*.log +local_settings.py +db.sqlite3 + +## Flask stuff: +instance/ +.webassets-cache + +## Scrapy stuff: +.scrapy + +## Sphinx documentation +docs/_build/ + +## PyBuilder +target/ + +## Jupyter Notebook +.ipynb_checkpoints + +## pyenv +.python-version + +## celery beat schedule file +celerybeat-schedule + +## SageMath parsed files +*.sage.py + +## Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +## Spyder project settings +.spyderproject +.spyproject + +## Rope project settings +.ropeproject + +## mkdocs documentation +/site + +## mypy +.mypy_cache/ + + + +# Java +## Compiled class file +*.class + +## Log file +*.log + +## BlueJ files +*.ctxt + +## Mobile Tools for Java (J2ME) +.mtj.tmp/ + +## Package Files +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +## virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + + + +# Android +## Built application files +*.apk +*.ap_ + +## Files for the ART/Dalvik VM +*.dex + +## Generated files +bin/ +gen/ +out/ + +## Gradle files +.gradle/ +build/ + +## Local configuration file (sdk path, etc) +local.properties + +## Proguard folder generated by Eclipse +proguard/ + +## Android Studio Navigation editor temp files +.navigation/ + +## Android Studio captures folder +captures/ + +## IntelliJ +*.iml +.idea/workspace.xml +.idea/tasks.xml +.idea/gradle.xml +.idea/assetWizardSettings.xml +.idea/dictionaries +.idea/libraries +.idea/caches + +## Keystore files +# Uncomment the following line if you do not want to check your keystore files in. +*.jks + +## External native build folder generated in Android Studio 2.2 and later +.externalNativeBuild + +## Google Services (e.g. APIs or Firebase) +google-services.json + +## Freeline +freeline.py +freeline/ +freeline_project_description.json + +## fastlane +fastlane/report.xml +fastlane/Preview.html +fastlane/screenshots +fastlane/test_output +fastlane/readme.md + + + +# CUDA +*.i +*.ii +*.gpu +*.ptx +*.cubin +*.fatbin + + + +# CMAKE +CMakeCache.txt +CMakeFiles +CMakeScripts +Testing +cmake_install.cmake +install_manifest.txt +compile_commands.json +CTestTestfile.cmake + + + +# QT +## Qt-es +object_script.*.Release +object_script.*.Debug +*_plugin_import.cpp +/.qmake.cache +/.qmake.stash +*.pro.user +*.pro.user.* +*.qbs.user +*.qbs.user.* +*.moc +moc_*.cpp +moc_*.h +qrc_*.cpp +ui_*.h +*.qmlc +*.jsc +### (reserved) Makefile* +*build-* + +## Qt unit tests +target_wrapper.* + +## QtCreator +*.autosave + +## QtCreator Qml +*.qmlproject.user +*.qmlproject.user.* + +## QtCreator CMake +CMakeLists.txt.user* + + + +# ETC. +## Ignore generated docs +*.dox +*.wikidoc + +## doxygen +doc/ + +## IDE: vscode +.vscode/ + +## IDE: Emacs +.#* + +## IDE: VIM +*.swp + +## Setting files +*.settings diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..cfba094 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,18 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/README.md b/README.md new file mode 100644 index 0000000..66d3304 --- /dev/null +++ b/README.md @@ -0,0 +1,220 @@ +

+ + GitHub + + + GitHub Workflow Status (with event) + + + GitHub Workflow Status (with event) + + + GitHub Workflow Status (with event) + + + GitHub issues + +

+ +

+ + PostGIS + + + PostGIS + +

+ +# PostGIS ROS2 Bridge + +ROS2 node connecting PostgreSQL databases containing spatial PostGIS data with the ROS world. + +## Basic Usage + +The publishing node can be started with + +````bash +ros2 run postgis_ros_bridge postgis_ros_bridge_publisher_node --ros-args --params-file /PATH_TO/params.yaml +```` +The `yaml` file is structured in two fixed sections (postgres and list of publisher), one optional default section, and n=len(list of publish) sections for each query. + +The `postgresql`-section with username / password (either as plain text or give the name of an environmen variable holding the password), as well as the hostname, port and the schema to be used: +````yaml +postgresql: + user: "postgres" + pass_env: "POSTGRES_PASSWORD" # read password from environment variable (recommended) + pass: "postgres" # Alternative to pass_env (store password in plaintext (not recommended)) + host: "localhost" + port: 5432 + schema: "example_schema" +```` +This is followed by a list of query publishers, for the example in `cfg/example.yaml`: +````yaml +publish: + - query_point + - query_pose + - query_pose_array + - query_pose_stamped + - query_polygon + - query_polygon_stamped + - query_marker + - query_marker_array + - query_pointcloud + +```` +For each of these list elements, there needs to be a section with parameters, depending on the type of desired message beeing published. + +The publisher(s) are set up in the defined sections from above. Every section has at least a SQL query that contains a column `geometry`. +Depending on the targeted message type, there is also a special column `rotation`, `frame_id`, and `id`. +Parameters for all sections can be set in a `query_defaults` section, e.g.: +````yaml +query_defaults: + rate: 10.0 + frame_id: "map" +```` + +## Geodesic Coordinates (UTM Transform) + +For simple use of geodesic coordinates (latitude, longitude) we offer a basic functionality to directly transform coordinates into a local cartesian coordinate frame (UTM). + +All queries where parameter `geodesic=True` are transformed according to the transformation defined in following configuration block: + +``` +cartesian_transform: + type: "utm" + # utm_zone: 33 + # utm_band: "N" + lon: 16.511422682732736 + lat: 47.977274686327114 + broadcast_cartesian_transform: true + yaw_offset: 0.0 + cartesian_frame_id: "utm" + world_frame_id: "map" +``` + +* `type` specify type of cartesian coordinate. Supported: `[utm]` +* `utm_zone`/`utm_band` directly set utm zone and band +* `lat`/`lon` used to auto detect utm zone / band if not directly set +* `broadcast_cartesian_transform`: broadcast the transformation between `[cartesian_frame_id]-->[world_frame_id]` +* `yaw_offset` manual rotational offset between cartesian and world frame +* `cartesian_frame_id` name of cartesian frame +* `world_frame_id` name of local map frame +* `inplace`: sets `lon`/`lat` as origin of our local map frame and directly transform points into this frame (broadcast must be disabled) + + +## Supported ROS2 Messages +* `geometry_msgs` + * `PointStamped` + * `Pose` + * `PoseArray` + * `PoseStamped` + * `Polygon` + * `PolygonStamped` +* `visualization_msgs` + * `Marker` +* `sensor_msgs` + * `PointCloud2` + +A simple example of to configure each type is listed in the following sections. + + +### geometry_msgs/msg/PointStamped [(ROS2 Reference)](https://docs.ros2.org/latest/api/geometry_msgs/msg/PointStamped.html) +````yaml +query_point: + query: "SELECT position AS geometry, 'test_frame_id' AS frame_id FROM landmark;" + type: "PointStamped" + topic: "point" +```` +This is a simple example to query the `position` column from a table called `landmark` and rename it to the defined `geometry` keyword. +The frame id is set static for all points to `test_frame_id`. This valued could be fetched from the database as well using a more advanced query. The type is set using the `type` parameter, and the topic to publish the result as `topic`. +The parameters set in the query_defaults (`rate` and `frame_id`) could be set as all here to overwrite the defaults. + +### geometry_msgs/msg/Pose [(ROS2 Reference)](https://docs.ros2.org/latest/api/geometry_msgs/msg/Pose.html) +````yaml +query_pose: + query: "SELECT pose.position AS geometry, pose.rotation_vector AS rotation FROM pose;" + type: "Pose" + topic: "pose" +```` +Query pose (x,y,z) in column `geometry` and rotation in column `rotation` as scaled euler rotation (see [scipy](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html)) in (roll, pitch, yaw). Each row of the query result is published as `Pose`. + +### geometry_msgs/msg/PoseArray [(ROS2 Reference)](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseArray.html) +````yaml +query_pose_array: + query: "SELECT pose.position AS geometry, pose.rotation_vector AS rotation FROM pose;" + type: "PoseArray" + topic: "pose_array" + frame_id: "test_frame_id" +```` +Query pose (x,y,z) in column `geometry` and rotation in column `rotation` as scaled euler rotation (see [scipy](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html)) in (roll, pitch, yaw). All poses of one query result are published as `PoseArray`. + +### geometry_msgs/msg/PoseStamped [(ROS2 Reference)](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseStamped.html) +````yaml +query_pose_stamped: + query: "SELECT pose.position AS geometry, pose.rotation_vector AS rotation, 'test_frame_id' AS frame_id FROM pose;" + type: "PoseStamped" + topic: "pose_stamped" +```` +Query pose (x,y,z) in column `geometry` and rotation in column `rotation` as scaled euler rotation (see [scipy](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html)) in (roll, pitch, yaw). Each row of the query result is published as `PoseStamped` with frame id and timestamp. + +### geometry_msgs/msg/Polygon [(ROS2 Reference)](https://docs.ros2.org/latest/api/geometry_msgs/msg/Polygon.html) +````yaml +query_polygon: + query: "SELECT ST_MakePolygon( 'LINESTRING(0 0, 5 5, 7.7 10, -1.0 10.0, 0 0)') AS geometry, 'test' AS frame_id;" + type: "Polygon" + topic: "polygon" +```` +Simple example of a 2D PostGIS polygon (could be fetched from a table too; 3D is also supported). The resulting geometry is published as `Polygon`. + +### geometry_msgs/msg/PolygonStamped [(ROS2 Reference)](https://docs.ros2.org/latest/api/geometry_msgs/msg/PolygonStamped.html) +````yaml +query_polygon_stamped: + query: "SELECT ST_MakePolygon( 'LINESTRING(0 0 0, 5 5 1, 7.7 10 1, -1.0 10.0 0, 0 0 0)') AS geometry, 'test' AS frame_id;" + type: "PolygonStamped" + topic: "polygon_stamped" + frame_id: "test_frame_id" +```` +Simple example of a 3D PostGIS polygon (could be fetched from a table too; 2D is also supported). The resulting geometry is published as `PolygonStamped` with frame id and timestamp. + +### visualization_msgs/msg/Marker [(ROS2 Reference)](https://docs.ros2.org/galactic/api/visualization_msgs/msg/Marker.html) +````yaml +query_marker: + query: "SELECT ROW_NUMBER() OVER (ORDER BY pose.id) AS id, pose.position AS geometry, pose.rotation_vector AS rotation, test_frame_id' AS frame_id FROM pose;" + type: "Marker" + marker_type: "visualization_msgs::Marker::SPHERE" # marker_type or here | default sphere + topic: "marker" +```` +Example of a marker message generation. As PostGIS does not support 6DoF poses, additionally to the `geometry` column containing the position (x,y,z) of the marker, a second column `rotation` is needed. This column is expected to hold the rotation as scaled euler rotation (see [scipy](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html)) in (roll, pitch, yaw). The `id` field of the ROS2 marker message can be filled for each row using the `id` column. +Implemented marker_types are `ARROW`, `CUBE`, `SPHERE`, and `CYLINDER` (list of supported types can be extended easily in code, see `postgis_ros_bridge/query_result_parser.py`). + +### visualization_msgs/msg/MarkerArray [(ROS2 Reference)](https://docs.ros2.org/galactic/api/visualization_msgs/msg/MarkerArray.html) +````yaml +query_marker_array: + query: "SELECT ROW_NUMBER() OVER (ORDER BY pose.id) AS id, pose.position AS geometry, pose.rotation_vector AS rotation FROM pose;" + type: "MarkerArray" + marker_type: "visualization_msgs::Marker::SPHERE" + topic: "marker_array" + frame_id: "test_frame_id" +```` +For suitable message types, there is also the `Array` version implemented. For this example, the type is simply set to `MarkerArray`. In this example, the `frame_id` is set in the parameter section. It is also possible to define the `frame_id` in the query itself or as here, in the defaults. Query result overwrite the value set in the query section, and this parameter overwrites one in the defaults section, i.e. query result -> parameter section -> default value. + +### sensor_mgsg/msg/PointCloud2 [(ROS2 Reference)](https://docs.ros2.org/latest/api/sensor_msgs/msg/PointCloud2.html) +````yaml +query_pointcloud: + query: "SELECT position AS geometry FROM landmark;" + type: "PointCloud2" + frame_id: "test_frame_id" + topic: "pointcloud" + rate: 1.0 +```` +Minimal query to fetch points to be published as pointcloud2. The `rate` parameter overwrites the default value. + + +# Funding +[DE] Die FFG ist die zentrale nationale Förderorganisation und stärkt Österreichs Innovationskraft. Dieses Projekt wird aus Mitteln der FFG gefördert. + +[EN] FFG is the central national funding organization and strengthens Austria's innovative power. This project is funded by the FFG. + +[www.ffg.at](www.ffg.at) + +Projekt: [openSCHEMA](https://iktderzukunft.at/de/projekte/open-semantic-collaborative-hierarchical-environment-mapping.php) \ No newline at end of file diff --git a/cfg/example.yaml b/cfg/example.yaml new file mode 100644 index 0000000..9f416f7 --- /dev/null +++ b/cfg/example.yaml @@ -0,0 +1,79 @@ +postgis_ros_publisher: + ros__parameters: + postgresql: + user: "postgres" + pass_env: "POSTGRES_PASSWORD" # read password from environment variable + pass: "postgres" # TODO: Alternative way w/o need of storing password in plaintext + host: "localhost" + port: 5432 + schema: "postgres_alchemy_ait" + + publish: + - query_point + - query_marker + - query_marker_array + - query_pointcloud + - query_pose + - query_pose_array + - query_pose_stamped + - query_polygon + - query_polygon_stamped + + # special section for default values for all queries + query_defaults: + rate: 10.0 + frame_id: "map" + + query_point: + query: "SELECT position AS geometry, 'test_frame_id' AS frame_id FROM landmark;" + type: "PointStamped" + topic: "point" + + query_marker: + query: "SELECT ROW_NUMBER() OVER (ORDER BY pose.id) AS id, pose.position AS geometry, pose.rotation_vector AS rotation, 'test_frame_id' AS frame_id FROM pose;" + type: "Marker" + marker_type: "visualization_msgs::Marker::SPHERE" # marker_type or here | default sphere + topic: "marker" + + query_marker_array: + query: "SELECT ROW_NUMBER() OVER (ORDER BY pose.id) AS id, pose.position AS geometry, pose.rotation_vector AS rotation, 'test_frame_id' AS frame_id FROM pose;" + type: "MarkerArray" + marker_type: "visualization_msgs::Marker::SPHERE" # marker_type or here | default sphere + topic: "marker_array" + frame_id: "test_frame_id" # custom + + query_pointcloud: + query: "SELECT position AS geometry FROM landmark;" + type: "PointCloud2" + frame_id: "test_frame_id" # custom + topic: "pointcloud" + rate: 1.0 + + query_pose: + query: "SELECT pose.position AS geometry, pose.rotation_vector AS rotation FROM pose;" + type: "Pose" + topic: "pose" + + query_pose_array: + query: "SELECT pose.position AS geometry, pose.rotation_vector AS rotation FROM pose;" + type: "PoseArray" + topic: "pose_array" + frame_id: "test_frame_id" + + query_pose_stamped: + query: "SELECT pose.position AS geometry, pose.rotation_vector AS rotation, 'test_frame_id' AS frame_id FROM pose;" + type: "PoseStamped" + topic: "pose_stamped" + + query_polygon: + query: "SELECT ST_MakePolygon( 'LINESTRING(0 0, 5 5, 7.7 10, -1.0 10.0, 0 0)') AS geometry, 'test' AS frame_id;" + type: "Polygon" + topic: "polygon" + + query_polygon_stamped: + query: "SELECT ST_MakePolygon( 'LINESTRING(0 0 0, 5 5 1, 7.7 10 1, -1.0 10.0 0, 0 0 0)') AS geometry, 'test' AS frame_id;" + type: "PolygonStamped" + topic: "polygon_stamped" + frame_id: "test_frame_id" + + diff --git a/cfg/test.yaml b/cfg/test.yaml new file mode 100644 index 0000000..42681bb --- /dev/null +++ b/cfg/test.yaml @@ -0,0 +1,85 @@ +postgis_ros_publisher: + ros__parameters: + postgresql: + user: "postgres" + pass_env: "POSTGRES_PASSWORD" # read password from environment variable + pass: "postgres" # TODO: Alternative way w/o need of storing password in plaintext + host: "localhost" + port: 5432 + schema: "postgres_alchemy_ait" + + publish: + - query_point + - query_marker + - query_marker_array + - query_pointcloud + - query_pose + - query_pose_array + - query_pose_stamped + - query_polygon + - query_polygon_stamped + + # special section for default values for all queries + query_defaults: + rate: 10.0 + frame_id: "map" + + query_point: + query: "SELECT position AS geometry, 'test_frame_id' AS frame_id FROM landmark;" + type: "PointStamped" + topic: "point" + + query_marker: + query: "SELECT ROW_NUMBER() OVER (ORDER BY pose.id) AS id, pose.position AS geometry, pose.rotation_vector AS rotation, 'test_frame_id' AS frame_id FROM pose;" + type: "Marker" + marker_type: "visualization_msgs::Marker::SPHERE" # marker_type or here | default sphere + topic: "marker" + + query_marker_array: + query: "SELECT ROW_NUMBER() OVER (ORDER BY pose.id) AS id, pose.position AS geometry, pose.rotation_vector AS rotation, 'test_frame_id' AS frame_id FROM pose;" + type: "MarkerArray" + marker_type: "visualization_msgs::Marker::SPHERE" # marker_type or here | default sphere + topic: "marker_array" + frame_id: "test_frame_id" # custom + + query_pointcloud: + query: "SELECT position AS geometry FROM landmark;" + type: "PointCloud2" + frame_id: "test_frame_id" # custom + topic: "pointcloud" + rate: 1.0 + + query_pose: + query: "SELECT pose.position AS geometry, pose.rotation_vector AS rotation FROM pose;" + type: "Pose" + topic: "pose" + + query_pose_array: + query: "SELECT pose.position AS geometry, pose.rotation_vector AS rotation FROM pose;" + type: "PoseArray" + topic: "pose_array" + frame_id: "test_frame_id" + + query_pose_stamped: + query: "SELECT pose.position AS geometry, pose.rotation_vector AS rotation, 'test_frame_id' AS frame_id FROM pose;" + type: "PoseStamped" + topic: "pose_stamped" + + query_polygon: + query: "SELECT ST_MakePolygon( 'LINESTRING(0 0, 5 5, 7.7 10, -1.0 10.0, 0 0)') AS geometry, 'test' AS frame_id;" + type: "Polygon" + topic: "polygon" + + + query_polygon_stamped: + query: "SELECT ST_MakePolygon( 'LINESTRING(0 0 0, 5 5 1, 7.7 10 1, -1.0 10.0 0, 0 0 0)') AS geometry, 'test' AS frame_id;" + type: "PolygonStamped" + topic: "polygon_stamped" + frame_id: "test_frame_id" + + + # frame_id: "map" + # rate: 10 +# "SELECT ST_MakePolygon( 'LINESTRING(75.15 29.53 1.0, 77 29 1,77.6 29.5 1.0 , 75.15 29.53 1.0 )') AS geometry, ST_GeometryType(ST_MakePolygon( 'LINESTRING(75.15 29.53 1,77 29 1,77.6 29.5 1, 75.15 29.53 1)')) AS type, 'test' AS frame_id;" +# "SELECT ST_MakePoint(10.15, 13.53, 0.0) AS geometry, ST_GeometryType(ST_MakePoint(10.15, 13.53, 0.0)) AS type, 'test' AS frame_id;" +# "SELECT ST_MakePoint(75.15, 29.53, 1.0) AS geometry, ST_GeometryType(ST_MakePoint(75.15, 29.53, 1.0)) AS type, 'test' AS frame_id;" diff --git a/cfg/test_utm.yaml b/cfg/test_utm.yaml new file mode 100644 index 0000000..512dd9f --- /dev/null +++ b/cfg/test_utm.yaml @@ -0,0 +1,91 @@ +postgis_ros_publisher: + ros__parameters: + + postgresql: + user: "postgres" + pass_env: "POSTGRES_PASSWORD" # read password from environment variable + pass: "postgres" # directly provides password (overwrites password from environment variable) + host: "L1AS17" + port: 5432 + schema: "test" + + publish: + - query_point + - query_marker + - query_road + - query_building + - query_pointcloud + - query_polygon + - query_polygon_stamped + + + # special section for default values for all queries + query_defaults: + frame_id: "map" + rate: 1.0 + geodesic: true + + # specify cartesian transformation for all queries with geodesic=true + cartesian_transform: + type: "utm" + # specify directly utm zone and band or alternatively provide a reference point with lon/lat + # utm_zone: 33 + # utm_band: "N" + lon: 16.511422682732736 + lat: 47.977274686327114 + broadcast_cartesian_transform: true + yaw_offset: 0.0 # manual rotation of the utm->map transform + world_frame_id: "map" + cartesian_frame_id: "utm" + inplace: false # directly transform utm to map without broadcasting tf + + + query_point: + query: "SELECT geom AS geometry FROM landmark;" + type: "PointStamped" + topic: "point" + geodesic: true + + query_road: + query: "SELECT id, geom AS geometry FROM road;" + topic: "marker_array" + type: "MarkerArray" + + # marker_type: "visualization_msgs::Marker::LINE_STRIP" + marker_scale: [1.0, 1.0, 1.0] + + + query_building: + query: "SELECT id, geom AS geometry FROM building;" + topic: "marker_array" + type: "MarkerArray" + # marker_type: "visualization_msgs::Marker::LINE_STRIP" + marker_ns: "buildings" + marker_color: [0.0, 1.0, 0.0, 1.0] + marker_scale: [1.0, 1.0, 1.0] + marker_lifetime: 1 # 1s + rate: 1.0 # publish once + + query_marker: + query: "SELECT id, geom AS geometry FROM landmark;" + type: "MarkerArray" # Point2D PointCloud, Polygon Marker, Marker, Pose + topic: "marker_array" + + marker_type: "visualization_msgs::Marker::SPHERE" + marker_ns: "landmarks" + marker_scale: [2.0, 2.0, 2.0] + + query_pointcloud: + query: "SELECT geom AS geometry FROM landmark;" + type: "PointCloud2" # Point2D PointCloud, Polygon Marker, Marker, Pose + topic: "pointcloud" + + query_polygon: + query: "SELECT ST_MakePolygon( 'LINESTRING(16.511422682732736 47.977274686327114 0, 16.511422682732736 47.977274686327114 2, 16.511422682732736 47.977296686329114 2 , 16.511422682732736 47.977274686327114 0)') AS geometry;" + type: "Polygon" + topic: "polygon" + + query_polygon_stamped: + query: "SELECT ST_MakePolygon( 'LINESTRING(16.511422682732736 47.977274686327114 0, 16.511422682732736 47.977274686327114 2, 16.511422682732736 47.977296686329114 2 , 16.511422682732736 47.977274686327114 0)') AS geometry;" + type: "PolygonStamped" + topic: "polygon_stamped" \ No newline at end of file diff --git a/docs/geodesic.md b/docs/geodesic.md new file mode 100644 index 0000000..d05dab7 --- /dev/null +++ b/docs/geodesic.md @@ -0,0 +1,7 @@ +# API Reference + +::: postgis_ros_bridge + options: + filters: [] + show_submodules: true + show_root_heading: true diff --git a/mkdocs.yml b/mkdocs.yml new file mode 100644 index 0000000..af8d2da --- /dev/null +++ b/mkdocs.yml @@ -0,0 +1,10 @@ +site_name: postgis_ros_bridge +site_url: https://example.com/ +theme: readthedocs + +plugins: +- simple +- mkdocstrings + +markdown_extensions: + - admonition diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..2749ccf --- /dev/null +++ b/package.xml @@ -0,0 +1,52 @@ + + + + postgis_ros_bridge + 0.0.0 + A library for publishing spatial PostGIS data to ROS. + Marco Wallner + Apache License 2.0 + Matthias Schoerghuber + Marco Wallner + Markus Hofstaetter + + sensor_msgs + sensor_msgs_py + geometry_msgs + visualization_msgs + builtin_interfaces + tf2_ros + rclpy + rcl_interfaces + python3-shapely + python3-scipy + libpq-dev + + + python3-pyproj + python3-sqlalchemy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + sensor_msgs + sensor_msgs_py + geometry_msgs + visualization_msgs + builtin_interfaces + tf2_ros + rclpy + rcl_interfaces + python3-shapely + python3-scipy + libpq-dev + + python3-pyproj + python3-sqlalchemy + + + ament_python + + diff --git a/postgis_ros_bridge/__init__.py b/postgis_ros_bridge/__init__.py new file mode 100644 index 0000000..7312422 --- /dev/null +++ b/postgis_ros_bridge/__init__.py @@ -0,0 +1 @@ +"""PostGIS to ROS bridge package.""" diff --git a/postgis_ros_bridge/geodesic_transform.py b/postgis_ros_bridge/geodesic_transform.py new file mode 100644 index 0000000..ea4dd69 --- /dev/null +++ b/postgis_ros_bridge/geodesic_transform.py @@ -0,0 +1,118 @@ +# Copyright 2023 AIT - Austrian Institute of Technology GmbH +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Module to transform a geodesic to a local cartesian frame.""" + +from typing import Tuple, SupportsFloat + +from pyproj import Transformer, Proj +from pyproj.aoi import AreaOfInterest +from pyproj.crs import CRS +from pyproj.database import query_utm_crs_info + +from geometry_msgs.msg import (Point, Point32) + + +class GeodesicTransform: + """Transform geodesic coordinates into a local cartesian frame.""" + + def __init__(self, crs_to, origin_transform: bool = False, origin_lon: float = 0.0, + origin_lat: float = 0.0) -> None: + """Initialize GeodesicTransform.""" + self.origin_easting = None + self.origin_northing = None + self.origin_transform = False + + crs_from = {"proj": 'latlong', "ellps": 'WGS84', "datum": 'WGS84'} + self.transformer = Transformer.from_crs( + crs_from, + crs_to + ) + self.proj = Proj(self.transformer.target_crs) + + """Set map origin for local cartesian frame.""" + # pylint: disable=unpacking-non-sequence + if origin_transform: + easing, northing = self.transformer.transform(origin_lon, origin_lat) + self.origin_easting = easing + self.origin_northing = northing + self.origin_transform = True + + @staticmethod + def to_utm(zone: int, band: str, **kwargs) -> "GeodesicTransform": + """Construct transfromer to UTM from fixed zone and band.""" + return GeodesicTransform({"proj": 'utm', "ellps": 'WGS84', "datum": 'WGS84', "zone": zone, + "band": band}, **kwargs) + + @staticmethod + def to_geocent(**kwargs) -> "GeodesicTransform": + """Construct transfromer to geocentric model.""" + return GeodesicTransform({"proj": 'geocent', "ellps": 'WGS84', "datum": 'WGS84'}, *kwargs) + + @staticmethod + def to_utm_lonlat(lon: float, lat: float, **kwargs) -> "GeodesicTransform": + """Construct transfromer to UTM for given lat/lon.""" + utm_crs_list = query_utm_crs_info( + datum_name="WGS84", + area_of_interest=AreaOfInterest( + west_lon_degree=lon, + south_lat_degree=lat, + east_lon_degree=lon, + north_lat_degree=lat, + ), + ) + utm_crs = CRS.from_epsg(utm_crs_list[0].code) + return GeodesicTransform(utm_crs, **kwargs, origin_lat=lat, origin_lon=lon) + + def transform_lonlat(self, lon: float, lat: float) -> Tuple[float, float]: + """Transform a point from lat/lon to local map and optionally apply offset.""" + # pylint: disable=unpacking-non-sequence + easting, northing = self.transformer.transform(lon, lat) + if self.origin_transform: + easting -= self.origin_easting + northing -= self.origin_northing + + return easting, northing + + def meridian_convergence(self, lon: float, lat: float) -> float: + """Return meridian convergence angle (rad) for given lon/lat.""" + factors = self.proj.get_factors(lon, lat) + return factors.meridian_convergence + + def transform_point(self, point: Point) -> Point: + """Transform a geodetic point to local cartesian frame.""" + easting, northing = self.transform_lonlat(lon=point.x, lat=point.y) + + return Point(x=easting, y=northing, z=point.z) + + def transform_point32(self, point: Point32) -> Point32: + """Transform a geodetic point to local cartesian frame.""" + easting, northing = self.transform_lonlat(lon=point.x, lat=point.y) + + return Point32(x=easting, y=northing, z=point.z) + + def transform_tuple( + self, point: Tuple[SupportsFloat, + SupportsFloat, + SupportsFloat]) -> Tuple[SupportsFloat, + SupportsFloat, + SupportsFloat]: + """Transform a geodetic point to local cartesian frame.""" + easting, northing = self.transform_lonlat(lon=point[0], lat=point[1]) + + return (easting, northing, point[2]) + + def __repr__(self) -> str: + """Return string representation of GeodesicTransform.""" + return f"{self.transformer}" diff --git a/postgis_ros_bridge/postgis_converter.py b/postgis_ros_bridge/postgis_converter.py new file mode 100644 index 0000000..7f7c043 --- /dev/null +++ b/postgis_ros_bridge/postgis_converter.py @@ -0,0 +1,155 @@ +# Copyright 2023 AIT - Austrian Institute of Technology GmbH +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""PostGIS converter module.""" +from typing import Union + +import shapely.geometry +from geometry_msgs.msg import (Point, Point32, Polygon, PolygonStamped, Pose, + PoseStamped, Quaternion) +from scipy.spatial.transform import Rotation +from shapely import wkb +from std_msgs.msg import Header +from visualization_msgs.msg import Marker + + +class PostGisConverter: + """PostGIS converter class.""" + + @staticmethod + def load(geometry: Union[bytes, str], as_hex=True): + """Load a shapely point from well known binary form.""" + return wkb.loads(geometry, hex=as_hex) + + @staticmethod + def to_point(geometry: Union[bytes, str], as_hex=True) -> Point: + """Convert a shapely point to a ROS point.""" + if not geometry: + return Point(x=0.0, y=0.0, z=0.0) + point = wkb.loads(geometry, hex=as_hex) + return PostGisConverter.to_point_(point) + + @staticmethod + def to_point_(point: shapely.geometry.Point) -> Point: + """Convert a shapely point to a ROS point.""" + return Point(x=point.x, y=point.y, z=point.z if point.has_z else 0.0) + + @staticmethod + def to_point_xyz(geometry: Union[bytes, str], as_hex=True): + """Convert a shapely point to a xyz tuple.""" + point = wkb.loads(geometry, hex=as_hex) + return (point.x, point.y, point.z if point.has_z else 0.0) + + @staticmethod + def to_orientation(orientation: Union[bytes, str], as_hex=True) -> Quaternion: + """Convert a orientation as well-known-binary to a ROS Quaternion.""" + # TODO: check quaternion order match with ROS + if not orientation: + return Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + orientation = wkb.loads(orientation, hex=as_hex) + return PostGisConverter.to_orientation_(orientation) + + @staticmethod + def to_orientation_(orientation: shapely.geometry.Point) -> Quaternion: + """Convert a orientation as shapeley point to a ROS Quaternion.""" + if not orientation: + return Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + quat = Rotation.from_rotvec( + [orientation.x, orientation.y, orientation.z]).as_quat() + return Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]) + + @staticmethod + def to_points_from_line_sring(geometry: shapely.geometry.LineString) -> list[Point]: + """Convert a shapely LineString to a list of ROS points.""" + return [Point(x=point[0], y=point[1], z=point[2] if geometry.has_z else 0.0) + for point in geometry.coords] + + @staticmethod + def to_points_from_polygon(geometry: shapely.geometry.LineString) -> list[Point]: + """Convert a shapely Polygon (as linestring) to a list of ROS points.""" + return [Point(x=point[0], y=point[1], z=point[2] if geometry.has_z else 0.0) + for point in geometry.coords] + + @staticmethod + def to_marker(header: Header, geometry: Union[bytes, str], + orientation: Union[bytes, str], as_hex, *args, **kwargs): + """Convert a shapely geometry to a ROS marker.""" + geometry = PostGisConverter.load(geometry, as_hex=as_hex) + geometry_orientation = PostGisConverter.load( + orientation, as_hex=as_hex) if orientation else None + + marker = Marker(header=header, *args, **kwargs) + + if geometry.geom_type == "Point": + marker.pose = PostGisConverter.to_pose_( + geometry, geometry_orientation) + elif geometry.geom_type == "LineString": + marker.points = PostGisConverter.to_points_from_line_sring( + geometry) + marker.type = Marker.LINE_STRIP + elif geometry.geom_type == "Polygon": + # TODO untested + marker.points = [Point(x=point[0], y=point[1], z=point[2] if geometry.has_z else 0.0) + for point in geometry.exterior.coords] + marker.type = Marker.LINE_STRIP + elif geometry.geom_type == "MultiPolygon": + # TODO: output only first polygon of multipolygon + geom = geometry.geoms[0] + marker.points = [Point(x=point[0], y=point[1], z=point[2] + if geometry.has_z else 0.0) for point in geom.exterior.coords] + marker.type = Marker.LINE_STRIP + elif geometry.geom_type == "MultiLineString": + geom = geometry.geoms[0] + marker.points = PostGisConverter.to_points_from_line_sring(geom) + marker.type = Marker.LINE_STRIP + else: + raise ValueError( + f"Unsupported geometry type: {geometry.geom_type}") + return marker + + @staticmethod + def to_pose(geometry: Union[bytes, str], orientation: Union[bytes, str], as_hex=True) -> Pose: + """Convert a point as well-known-binary to a ROS Pose.""" + geometry_position = PostGisConverter.load(geometry, as_hex=as_hex) + geometry_orientation = PostGisConverter.load(orientation, as_hex=as_hex) + return PostGisConverter.to_pose_(point=geometry_position, + orientation=geometry_orientation) + + @staticmethod + def to_pose_(point: shapely.geometry.Point, orientation: shapely.geometry.Point) -> Pose: + """Convert a point as shapely point to a ROS Pose.""" + return Pose(position=PostGisConverter.to_point_(point), + orientation=PostGisConverter.to_orientation_(orientation)) + + @staticmethod + def to_pose_stamped(header: Header, geometry: Union[bytes, str], + orientation: Union[bytes, str], as_hex=True) -> PoseStamped: + """Convert a point as well-known-binary to a ROS PoseStamped.""" + pose = PostGisConverter.to_pose(geometry, orientation, as_hex=as_hex) + return PoseStamped(header=header, pose=pose) + + @staticmethod + def to_polygon(geometry: Union[bytes, str], as_hex=True) -> Polygon: + """Convert a polygon as well-known-binary to a ROS Polygon.""" + polygon = wkb.loads(geometry, hex=as_hex) + return Polygon( + points=[Point32(x=point[0], y=point[1], z=point[2] if polygon.has_z else 0.0) + for point in polygon.boundary.coords]) + + @staticmethod + def to_polygon_stamped(header: Header, + geometry: Union[bytes, str], as_hex=True) -> PolygonStamped: + """Convert a polygon as well-known-binary to a ROS PolygonStamped.""" + polygon = PostGisConverter.to_polygon(geometry, as_hex=as_hex) + return PolygonStamped(header=header, polygon=polygon) diff --git a/postgis_ros_bridge/postgis_ros_bridge_publisher_node.py b/postgis_ros_bridge/postgis_ros_bridge_publisher_node.py new file mode 100644 index 0000000..548488b --- /dev/null +++ b/postgis_ros_bridge/postgis_ros_bridge_publisher_node.py @@ -0,0 +1,278 @@ +# Copyright 2023 AIT - Austrian Institute of Technology GmbH +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""PostGIS ROS Bridge Publisher Node.""" +from functools import partial +from typing import Dict, Tuple +import math + +import rclpy +from rclpy.node import Node +from rclpy.publisher import Publisher +from geometry_msgs.msg import PoseArray, TransformStamped +from visualization_msgs.msg import MarkerArray +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +from postgis_ros_bridge.postgresql_connection import PostgreSQLConnection +from postgis_ros_bridge.query import Query +from postgis_ros_bridge.query_result_parser import ( + BasicStampedArrayParserFactory, MarkerResultParser, PC2ResultParser, + PointResultParser, PolygonResultParser, PolygonStampedResultParser, + PoseResultParser, PoseStampedResultParser, QueryResultDefaultParameters, + QueryResultParser) + +from postgis_ros_bridge.geodesic_transform import GeodesicTransform + + +query_parser: Dict[str, QueryResultParser] = { + q.TYPE: q for q in [ + PointResultParser, + PoseResultParser, + PoseStampedResultParser, + PC2ResultParser, + MarkerResultParser, + PolygonResultParser, + PolygonStampedResultParser + ]} + +query_parser.update({ + "MarkerArray": + BasicStampedArrayParserFactory.create_array_parser( + MarkerResultParser, MarkerArray, "markers"), + "PoseArray": + BasicStampedArrayParserFactory.create_array_parser( + PoseResultParser, PoseArray, "poses"), +}) + + +def euler_to_quaternion(roll: float, + pitch: float, + yaw: float) -> Tuple[float, float, float, float]: + """Convert euler angles (roll-pitch-yaw) in radians to quaternion (x,y,z,w).""" + cos_y = math.cos(yaw * 0.5) + sin_y = math.sin(yaw * 0.5) + cos_p = math.cos(pitch * 0.5) + sin_p = math.sin(pitch * 0.5) + cos_r = math.cos(roll * 0.5) + sin_r = math.sin(roll * 0.5) + + quat_w = cos_r * cos_p * cos_y + sin_r * sin_p * sin_y + quat_x = sin_r * cos_p * cos_y - cos_r * sin_p * sin_y + quat_y = cos_r * sin_p * cos_y + sin_r * cos_p * sin_y + quat_z = cos_r * cos_p * sin_y - sin_r * sin_p * cos_y + + return (quat_x, quat_y, quat_z, quat_w) + + +class PostGisPublisher(Node): + """PostGIS ROS Bridge Publisher Node.""" + + def __init__(self): + super().__init__(node_name="postgis_ros_publisher") + self.get_logger().info(f"Starting {self.get_name()}...") + + # automatically_declare_parameters_from_overrides=True + self.declare_parameters( + namespace="", + parameters=[ + ("publish", rclpy.Parameter.Type.STRING_ARRAY), + ], + ) + + configurations = self.get_parameter("publish").value + self.get_logger().info(f"Parsing sections: {configurations}") + self.converter_pubs: Dict[str, Publisher] = dict() + self.postgresql_connection = PostgreSQLConnection(self) + self.get_logger().info( + f"Connected to database via {self.postgresql_connection}") + + # get common default settings + default_section_name = "query_defaults" + default_parameters = QueryResultDefaultParameters() + self.declare_parameters( + namespace="", + parameters=list(map(lambda x: (f"{default_section_name}.{x[0]}", x[1], x[2]), + default_parameters.declare_params()))) + default_parameters.set_params( + self.get_parameters_by_prefix(default_section_name)) + + self.geodesic_transformer = None + self.init_geodesic_transformer() + + for config in configurations: + self.declare_parameters( + namespace="", + parameters=[ + (f"{config}.query", rclpy.Parameter.Type.STRING), + (f"{config}.rate", default_parameters.rate), + (f"{config}.type", rclpy.Parameter.Type.STRING), + ], + ) + query_type = self.get_parameter(f"{config}.type").value + sql_query = self.get_parameter(f"{config}.query").value + rate = self.get_parameter(f"{config}.rate").value + + if query_type not in query_parser.keys(): + raise ValueError( + f"Type: '{query_type}' is not supported. Supported: {query_parser.keys()}" + ) + + parser = query_parser[query_type]() + # TODO: namespace bug in rclpy Node declare + # params name initialized after value query [fixme] + self.declare_parameters( + namespace="", parameters=list( + map(lambda x: (f"{config}.{x[0]}", x[1], x[2]), + parser.declare_params(default_parameters)))) + topics_msgs = parser.set_params( + self.get_parameters_by_prefix(config)) + + if parser.geodesic: + if self.geodesic_transformer is None: + raise ValueError( + "Geodesic transform is enabled but no cartesian transform is set") + parser.set_geodesic_transformer(self.geodesic_transformer) + + query = Query(self.postgresql_connection, sql_query) + + # TODO: probably sensor data qos or later configurable [fixme] + pubs = [(t, self.create_publisher(m, t, 10)) + for (t, m) in topics_msgs] + self.converter_pubs.update(pubs) + + self.get_logger().info(f"Register parser: {str(parser)}") + + if rate > 0: + self.create_timer( + 1.0/rate, partial(self.timer_callback, query, parser)) + else: + self.timer_callback(query, parser) + + def init_geodesic_transformer(self): + """Initialize cartesian transformation.""" + config = "cartesian_transform" + self.declare_parameters( + namespace="", + parameters=[ + (f"{config}.type", ""), + (f"{config}.utm_zone", -1), + (f"{config}.utm_band", ""), + (f"{config}.lon", -1000.0), + (f"{config}.lat", -1000.0), + (f"{config}.yaw_offset", 0.0), + (f"{config}.broadcast_cartesian_transform", False), + (f"{config}.world_frame_id", "map"), + (f"{config}.cartesian_frame_id", "utm"), + (f"{config}.inplace", False), + ], + ) + + self.geodesic_transformer = None + + cartesian_type = self.get_parameter(f"{config}.type").value + # if no cartesian transform type is set, disable cartesian transform + if cartesian_type == "": + return + + utm_zone = self.get_parameter(f"{config}.utm_zone").value + utm_band = self.get_parameter(f"{config}.utm_band").value + lon = self.get_parameter(f"{config}.lon").value + lat = self.get_parameter(f"{config}.lat").value + broadcast = self.get_parameter(f"{config}.broadcast_cartesian_transform").value + yaw_offset = self.get_parameter(f"{config}.yaw_offset").value + world_frame_id = self.get_parameter(f"{config}.world_frame_id").value + cartesian_frame_id = self.get_parameter(f"{config}.cartesian_frame_id").value + inplace = self.get_parameter(f"{config}.inplace").value + + if (inplace or broadcast) and (lon == -1000.0 or lat == -1000.0): + raise ValueError("lon and lat must be set if inplace or broadcast is True") + + if cartesian_type == "utm": + if utm_zone > -1: + if utm_band == "": + raise ValueError("utm_band is not set") + + self.geodesic_transformer = GeodesicTransform.to_utm( + utm_zone, utm_band, origin_transform=inplace, origin_lon=lon, origin_lat=lat) + else: + + if lon == -1000.0 or lat == -1000.0: + raise ValueError("lon and lat must be set if no utm_zone/utm_band is set") + + self.geodesic_transformer = GeodesicTransform.to_utm_lonlat( + lon, lat, origin_transform=inplace) + + else: + raise ValueError( + f"Type: '{cartesian_type}' is not supported. Supported: utm" + ) + + self.get_logger().info(f"Initialized cartesian transform: \ + {str(self.geodesic_transformer)}") + + if broadcast: + if inplace: + raise ValueError("inplace and broadcast can not be both True") + + self.tf_static_broadcaster = StaticTransformBroadcaster(self) + + tf_msg = TransformStamped() + + tf_msg.header.stamp = self.get_clock().now().to_msg() + tf_msg.header.frame_id = cartesian_frame_id + tf_msg.child_frame_id = world_frame_id + + easting, northing = self.geodesic_transformer.transform_lonlat(lon, lat) + tf_msg.transform.translation.x = easting + tf_msg.transform.translation.y = northing + tf_msg.transform.translation.z = 0.0 + + meridian_convergence = self.geodesic_transformer.meridian_convergence(lon, lat) + yaw = meridian_convergence + yaw_offset + quat = euler_to_quaternion(0.0, 0.0, math.radians(yaw)) + + tf_msg.transform.rotation.x = quat[0] + tf_msg.transform.rotation.y = quat[1] + tf_msg.transform.rotation.z = quat[2] + tf_msg.transform.rotation.w = quat[3] + + self.get_logger().info(f"Broadcasting cartesian transform\n\ + [{tf_msg.header.frame_id}]->[{tf_msg.child_frame_id}]\n\ + translation: x={tf_msg.transform.translation.x}\n\ + y={tf_msg.transform.translation.y}\n\ + z={tf_msg.transform.translation.z}\n\ + meridian_convergence: {meridian_convergence} deg\n\ + yaw_offset: {yaw_offset} deg") + + self.tf_static_broadcaster.sendTransform(tf_msg) + + def timer_callback(self, query: Query, converter: QueryResultParser): + """Timer callback for all queries.""" + with query.get_results() as results: + for topic, message in converter.parse_result(results, self.get_clock().now().to_msg()): + self.converter_pubs[topic].publish(message) + + +def main(args=None): + """ROS main function.""" + rclpy.init(args=args) + postgis_publisher = PostGisPublisher() + rclpy.spin(postgis_publisher) + + postgis_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/postgis_ros_bridge/postgresql_connection.py b/postgis_ros_bridge/postgresql_connection.py new file mode 100644 index 0000000..8fb5ea6 --- /dev/null +++ b/postgis_ros_bridge/postgresql_connection.py @@ -0,0 +1,65 @@ +# Copyright 2023 AIT - Austrian Institute of Technology GmbH +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""PostgreSQL Connection Abstraction class.""" +import os +from contextlib import AbstractContextManager + +from rclpy.node import Node +from sqlalchemy import create_engine + + +class PostgreSQLConnection(AbstractContextManager): + """PostgreSQL Connection Abstraction class.""" + + def __init__(self, node: Node): + namespace = "postgresql" + node.declare_parameters( + namespace="", + parameters=[ + (f"{namespace}.user", "postgres"), + (f"{namespace}.pass", ""), + (f"{namespace}.pass_env", ""), + (f"{namespace}.host", "localhost"), + (f"{namespace}.port", 5432), + (f"{namespace}.schema", "public"), + ], + ) + + user = node.get_parameter(f"{namespace}.user").value + passwd = node.get_parameter(f"{namespace}.pass").value + if not passwd: + passwd_env = node.get_parameter(f"{namespace}.pass_env").value + try: + passwd = os.environ[passwd_env] + except KeyError as ex: + raise ValueError( + f"Environment variable '{passwd_env}' is not set.") from ex + + host = node.get_parameter(f"{namespace}.host").value + port = node.get_parameter(f"{namespace}.port").value + schema = node.get_parameter(f"{namespace}.schema").value + + connection_uri = f"postgresql://{user}:{passwd}@{host}:{port}/{schema}" + self.engine = create_engine(connection_uri, execution_options={ + 'postgresql_readonly': True}) + + def __enter__(self): + return self + + def __exit__(self, _type, _value, _traceback): + self.engine.dispose() + + def __repr__(self) -> str: + return f"{str(self.engine.url)}" diff --git a/postgis_ros_bridge/query.py b/postgis_ros_bridge/query.py new file mode 100644 index 0000000..fc96dfd --- /dev/null +++ b/postgis_ros_bridge/query.py @@ -0,0 +1,43 @@ +# Copyright 2023 AIT - Austrian Institute of Technology GmbH +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Query class for executing SQL queries on the database.""" +from contextlib import AbstractContextManager + +from sqlalchemy import Result, text +from sqlalchemy.orm import sessionmaker + +from postgis_ros_bridge.postgresql_connection import PostgreSQLConnection + + +class Query(AbstractContextManager): + """Query class for executing SQL queries on the database.""" + + def __init__(self, postgresql_conn: PostgreSQLConnection, query: str): + session_ = sessionmaker(bind=postgresql_conn.engine) + self._session = session_() + self._query = text(query) + + def __enter__(self): + return self + + def __exit__(self, _type, _value, _traceback): + self._session.close() + + def get_results(self) -> Result: + """Execute the query and return the results.""" + return self._session.execute(self._query) + + def __repr__(self) -> str: + return super().__repr__() + f"({self._query})" diff --git a/postgis_ros_bridge/query_result_parser.py b/postgis_ros_bridge/query_result_parser.py new file mode 100644 index 0000000..5fdfe3d --- /dev/null +++ b/postgis_ros_bridge/query_result_parser.py @@ -0,0 +1,441 @@ +# Copyright 2023 AIT - Austrian Institute of Technology GmbH +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Parser classes for converting query results to ROS messages.""" +from abc import ABC, abstractmethod +from typing import Any, Dict, Iterable, Tuple + +from builtin_interfaces.msg import Duration, Time +from geometry_msgs.msg import (PointStamped, Polygon, PolygonStamped, + Pose, PoseStamped, Vector3, Quaternion) +from rcl_interfaces.msg import ParameterDescriptor +from rclpy.parameter import Parameter +from sensor_msgs.msg import PointCloud2 +from sensor_msgs_py import point_cloud2 +from sqlalchemy import Result, Row +from std_msgs.msg import ColorRGBA, Header +from visualization_msgs.msg import Marker + +from postgis_ros_bridge.postgis_converter import PostGisConverter +from postgis_ros_bridge.geodesic_transform import GeodesicTransform + + +class QueryResultDefaultParameters: + """Default parameters for query result parsers.""" + + def __init__(self): + self._rate = None + self._frame_id = None + self._geodesic = None + + def declare_params(self) -> Iterable[Tuple[str, Any]]: + """Declare default parameters for query result parsers.""" + return [ + ('rate', 1.0, ParameterDescriptor()), + ('frame_id', 'map', ParameterDescriptor()), + ('geodesic', False, ParameterDescriptor()), + ] + + def set_params(self, params: Dict[str, Parameter]) -> Iterable[Tuple[str, Any]]: + """Set default parameters for query result parsers.""" + self._rate = params['rate'].value + self._frame_id = params['frame_id'].value + self._geodesic = params['geodesic'].value + + @property + def rate(self): + """Get rate of query result parser.""" + return self._rate + + @property + def frame_id(self): + """Get frame id of query result parser.""" + return self._frame_id + + @property + def geodesic(self): + """Get utm transform enabled / disabled of query result parser.""" + return self._geodesic + + +class QueryResultParser(ABC): + """Abstract Query Result Parser.""" + + TYPE = "AbstractParser" + + @abstractmethod + def declare_params(self, + defaults: QueryResultDefaultParameters) -> Iterable[ + Tuple[str, Any, ParameterDescriptor]]: + """Define API of declare_params.""" + return [] + + @abstractmethod + def set_params(self, params: Dict[str, Parameter]) -> Iterable[Tuple[str, Any]]: + """Define API of set_params.""" + return [] + + @abstractmethod + def parse_result(self, result: Result, time: Time) -> Iterable[Tuple[str, Any]]: + """Define API of parse_result.""" + return [] + + def __repr__(self) -> str: + return self.TYPE + + +class StampedTopicParser(QueryResultParser): + """Base class for parsers which produce a single stamped message topic.""" + + TYPE = None + + def __init__(self) -> None: + """Initialize StampedTopicParser.""" + super().__init__() + self.frame_id = None + self.topic = None + self.geodesic = None + self.geodesic_transformer = None + + def declare_params( + self, + defaults: QueryResultDefaultParameters) -> Iterable[Tuple[str, + Any, + ParameterDescriptor]]: + """Implement API of declare_params.""" + return [ + ('frame_id', defaults.frame_id, ParameterDescriptor()), + ('topic', Parameter.Type.STRING, ParameterDescriptor()), + ('geodesic', defaults.geodesic, ParameterDescriptor()), + ] + + def set_geodesic_transformer(self, transformer: GeodesicTransform): + """Set cartesian transfromer.""" + self.geodesic_transformer = transformer + + def set_params(self, params: Dict[str, Parameter]) -> Iterable[Tuple[str, Any]]: + """Implement API of set_params.""" + self.frame_id = params['frame_id'].value + self.topic = params['topic'].value + self.geodesic = params['geodesic'].value + + return [] + + def get_frame_id(self, elem: Row) -> str: + """Get frame id of from query.""" + return elem.frame_id if hasattr(elem, 'frame_id') else self.frame_id + + +class SingleElementParser(StampedTopicParser): + """Base class for parsers which produce a single stamped message topic.""" + + TYPE = None + + @abstractmethod + def parse_single_element(self, element: Row, time: Time) -> Tuple[str, Any]: + """Implement API of parse_single_element.""" + return None + + def parse_result(self, result: Result, time: Time) -> Iterable[Tuple[str, Any]]: + """Implement API of parse_result for single element parsers.""" + return (self.parse_single_element(element, time) for element in result) + + +class PointResultParser(SingleElementParser): + """Parser for point results.""" + + TYPE = "PointStamped" + + def set_params(self, params: Dict[str, Parameter]) -> Iterable[Tuple[str, Any]]: + """Implement API of set_params for point results.""" + return super().set_params(params) + [(self.topic, PointStamped)] + + def parse_single_element(self, element: Row, time: Time) -> Tuple[str, Any]: + """Implement API of parse_single_element for point results.""" + msg = PointStamped(header=Header(frame_id=self.get_frame_id(element), stamp=time), + point=PostGisConverter.to_point(element.geometry)) + + if self.geodesic: + msg.point = self.geodesic_transformer.transform_point(msg.point) + + return (self.topic, msg) + + def __repr__(self) -> str: + return super().__repr__() + f" (using frame_id: {self.frame_id} and topic: {self.topic})" + + +class PoseResultParser(SingleElementParser): + """Parser for pose results.""" + + TYPE = "Pose" + + def set_params(self, params: Dict[str, Parameter]) -> Iterable[Tuple[str, Any]]: + """Implement API of set_params for pose results.""" + return super().set_params(params) + [(self.topic, Pose)] + + def parse_single_element(self, element: Row, time: Time) -> Tuple[str, Any]: + """Implement API of parse_single_element for pose results.""" + msg = PostGisConverter.to_pose(element.geometry, element.rotation) + + if self.geodesic_transformer: + msg.position = self.geodesic_transformer.transform_point(msg.position) + + return (self.topic, msg) + + def __repr__(self) -> str: + return super().__repr__() + f" (using topic: {self.topic})" + + +class PoseStampedResultParser(SingleElementParser): + """Parser for pose stamped results.""" + + TYPE = "PoseStamped" + + def set_params(self, params: Dict[str, Parameter]) -> Iterable[Tuple[str, Any]]: + """Implement API of set_params for pose stamped results.""" + return super().set_params(params) + [(self.topic, PoseStamped)] + + def parse_single_element(self, element: Row, time: Time) -> Tuple[str, Any]: + """Implement API of parse_single_element for pose stamped results.""" + msg = PostGisConverter.to_pose_stamped(geometry=element.geometry, + orientation=element.rotation, + header=Header(frame_id=self.get_frame_id(element), + stamp=time)) + if self.geodesic_transformer: + # TODO: handle orientation # pylint: disable=fixme + msg.pose.position = self.geodesic_transformer.transform_point( + msg.pose.position) + return (self.topic, msg) + + def __repr__(self) -> str: + return super().__repr__() + f" (using frame_id: {self.frame_id} and topic: {self.topic})" + + +class PC2ResultParser(StampedTopicParser): + """Parser for pointcloud2 results.""" + + TYPE = "PointCloud2" + + def set_params(self, params: Dict[str, Parameter]) -> Iterable[Tuple[str, Any]]: + """Implement API of set_params for pointcloud2 results.""" + return super().set_params(params) + [(self.topic, PointCloud2)] + + def parse_result(self, result: Result, time: Time) -> Iterable[Tuple[str, Any]]: + """Implement API of parse_result for pointcloud2 results.""" + pointcloud_msg = PointCloud2() + header = Header(frame_id=self.frame_id, stamp=time) + points = [ + PostGisConverter.to_point_xyz(element.geometry) for element in result.all() + ] + + if self.geodesic_transformer: + points = [self.geodesic_transformer.transform_tuple( + point) for point in points] + + pointcloud_msg = point_cloud2.create_cloud_xyz32(header, points) + return [(self.topic, pointcloud_msg)] + + def __repr__(self) -> str: + return super().__repr__() + f" (using frame_id: {self.frame_id} and topic: {self.topic})" + + +class PolygonResultParser(SingleElementParser): + """Parser for polygon results.""" + + TYPE = "Polygon" + + def set_params(self, params: Dict[str, Parameter]) -> Iterable[Tuple[str, Any]]: + """Implement API of set_params for polygon results.""" + return super().set_params(params) + [(self.topic, Polygon)] + + def parse_single_element(self, element: Row, time: Time) -> Tuple[str, Any]: + """Implement API of parse_single_element for polygon results.""" + msg = PostGisConverter.to_polygon(element.geometry) + + if self.geodesic_transformer: + msg.points = [self.geodesic_transformer.transform_point32( + point) for point in msg.points] + + return (self.topic, msg) + + def __repr__(self) -> str: + return super().__repr__() + f" (using topic: {self.topic})" + + +class PolygonStampedResultParser(SingleElementParser): + """Parser for polygon stamped results.""" + + TYPE = "PolygonStamped" + + def set_params(self, params: Dict[str, Parameter]) -> Iterable[Tuple[str, Any]]: + """Implement API of set_params for polygon stamped results.""" + return super().set_params(params) + [(self.topic, PolygonStamped)] + + def parse_single_element(self, element: Row, time: Time) -> Tuple[str, Any]: + """Implement API of parse_single_element for polygon stamped results.""" + msg = PostGisConverter.to_polygon_stamped( + geometry=element.geometry, + header=Header(frame_id=self.get_frame_id(element), + stamp=time)) + + if self.geodesic_transformer: + msg.polygon.points = [self.geodesic_transformer.transform_point32( + point) for point in msg.polygon.points] + + return (self.topic, msg) + + def __repr__(self) -> str: + return super().__repr__() + f" (using topic: {self.topic})" + + +class MarkerResultParser(SingleElementParser): + """Parser for marker results.""" + + TYPE = "Marker" + + def __init__(self): + super().__init__() + self.marker_type = None + self.marker_ns = None + self.marker_color = None + self.marker_scale = None + self.marker_lifetime = None + + def declare_params( + self, + defaults: QueryResultDefaultParameters) -> Iterable[Tuple[str, Any, + ParameterDescriptor]]: + """Implement API of declare_params for marker results.""" + return super().declare_params(defaults) + [ + ('marker_type', Parameter.Type.STRING, ParameterDescriptor()), + ('marker_ns', Parameter.Type.STRING, ParameterDescriptor()), + ('marker_color', Parameter.Type.DOUBLE_ARRAY, ParameterDescriptor()), + ('marker_scale', Parameter.Type.DOUBLE_ARRAY, ParameterDescriptor()), + ('marker_lifetime', 3, ParameterDescriptor()), + ('marker_mesh_resource', "", ParameterDescriptor()), + ('marker_orientation', Parameter.Type.DOUBLE_ARRAY, ParameterDescriptor()), + ] + + def set_params(self, params: Dict[str, Parameter]) -> Iterable[Tuple[str, Any]]: + """Implement API of set_params for marker results.""" + topics = super().set_params(params) + self.marker_type = params['marker_type'].value + self.marker_ns = params['marker_ns'].value if params['marker_ns'].value else '' + self.marker_color = params['marker_color'].value if params['marker_color'].value else [ + 1.0, 0.0, 0.0, 1.0] + self.marker_scale = params['marker_scale'].value if params['marker_scale'].value else [ + 0.1, 0.1, 0.1] + self.marker_lifetime = Duration(sec=params['marker_lifetime'].value) + self.marker_mesh_resource = params['marker_mesh_resource'].value + self.marker_orientation = params['marker_orientation'].value + self.marker_orientation_override = True if self.marker_orientation else False + + return topics + [(self.topic, Marker)] + + def parse_single_element(self, element: Row, time: Time) -> Tuple[str, Any]: + """Implement API of parse_single_element for marker results.""" + def get_id(elem): + """Get id of element if it has one, otherwise return 0.""" + return elem.id if hasattr(elem, 'id') else 0 + + marker_color = ColorRGBA(r=self.marker_color[0], + g=self.marker_color[1], + b=self.marker_color[2], + a=self.marker_color[3]) + marker_types = { + "visualization_msgs::Marker::ARROW": Marker.ARROW, + "visualization_msgs::Marker::CUBE": Marker.CUBE, + "visualization_msgs::Marker::SPHERE": Marker.SPHERE, + "visualization_msgs::Marker::CYLINDER": Marker.CYLINDER, + "visualization_msgs::Marker::MESH_RESOURCE": Marker.MESH_RESOURCE, + } + marker_type = marker_types.get(self.marker_type, Marker.ARROW) + marker_scale = Vector3( + x=self.marker_scale[0], y=self.marker_scale[1], z=self.marker_scale[2]) + + msg = PostGisConverter.to_marker(header=Header(frame_id=self.get_frame_id(element), + stamp=time), + as_hex=True, + geometry=element.geometry, + orientation=None, + action=Marker.MODIFY, + id=get_id(element), + ns=self.marker_ns, + type=marker_type, + scale=marker_scale, + color=marker_color, + mesh_resource=self.marker_mesh_resource, + lifetime=self.marker_lifetime) + + if self.marker_orientation_override: + msg.pose.orientation = Quaternion(x=self.marker_orientation[0], + y=self.marker_orientation[1], + z=self.marker_orientation[2], + w=self.marker_orientation[3]) + + if self.geodesic_transformer: + # todo handle orientation + if msg.type in [Marker.LINE_STRIP, Marker.LINE_LIST, Marker.POINTS]: + msg.points = [self.geodesic_transformer.transform_point( + point) for point in msg.points] + else: + msg.pose.position = self.geodesic_transformer.transform_point( + msg.pose.position) + + return (self.topic, msg) + + def __repr__(self) -> str: + return super().__repr__() + f" (using frame_id: {self.frame_id} and topic: {self.topic})" + + +class BasicStampedArrayParserFactory: + """Factory for basic stamped array parsers.""" + + @staticmethod + def create_array_parser(cls: SingleElementParser, msg: Any, field: str): + """Create array parser for given message and field.""" + class ArrayParserMessage(cls): + """Array parser for given message and field.""" + + def __init__(self) -> None: + super().__init__() + self._has_header = hasattr(msg, 'header') + + def set_params(self, params: Dict[str, Parameter]) -> Iterable[Tuple[str, Any]]: + """Implement API of set_params for array parser.""" + super().set_params(params) + return [(self.topic, msg)] + + def parse_result(self, result: Result, time: Time) -> Iterable[Tuple[str, Any]]: + """Implement API of parse_result for array parser.""" + # TODO: toplevel header (frame_id, timestamp can be different + # from internal elements). + # Add addition toplevel param and if not defined use header of first element. + args = {} + + if self._has_header: + args['header'] = Header( + frame_id=self.get_frame_id(None), stamp=time) + + args.update({field: [self.parse_single_element( + element, time)[1] for element in result]}) + + message = msg(**args) + return [(self.topic, message)] + + def __repr__(self) -> str: + return f"{super().TYPE}Array "\ + f"(using frame_id: {self.frame_id} and topic: {self.topic})" + + return ArrayParserMessage diff --git a/resource/postgis_ros_bridge b/resource/postgis_ros_bridge new file mode 100644 index 0000000..e69de29 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..bbf6035 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/postgis_ros_bridge +[install] +install_scripts=$base/lib/postgis_ros_bridge diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..e6a5389 --- /dev/null +++ b/setup.py @@ -0,0 +1,41 @@ +from setuptools import setup + +package_name = 'postgis_ros_bridge' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=[ + 'setuptools', + 'sqlalchemy>=2.0.0', + 'psycopg2', + 'pytest-postgresql', + ], + zip_safe=True, + maintainer='vscode', + maintainer_email='marco.wallner@ait.ac.at', + description='A library for publishing spatial PostGIS data to ROS.', + license='Apache Software License', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'postgis_ros_bridge_publisher_node = ' + 'postgis_ros_bridge.postgis_ros_bridge_publisher_node:main' + ], + }, + classifiers=[ + 'License :: OSI Approved :: Apache Software License', + 'Programming Language :: Python :: 3 :: Only', + 'Programming Language :: Python :: 3.10', + 'Topic :: Database :: Front-Ends', + 'Topic :: Scientific/Engineering :: GIS', + 'Topic :: Software Development :: Libraries :: Python Modules', + ], + python_requires='>=3.10', +) diff --git a/test/__init__.py b/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/test/sql_data/postgis_test.sql b/test/sql_data/postgis_test.sql new file mode 100644 index 0000000..1cd646b --- /dev/null +++ b/test/sql_data/postgis_test.sql @@ -0,0 +1,40 @@ +-- Enable PostGIS (includes raster) +CREATE EXTENSION IF NOT EXISTS postgis; + +-- Create the tables +CREATE TABLE public.point ( + id SERIAL PRIMARY KEY, + geom GEOMETRY(PointZ, 4326) +); + +CREATE TABLE public.linestring ( + id SERIAL PRIMARY KEY, + geom GEOMETRY(LineStringZ, 4326) +); + +CREATE TABLE public.polygon ( + id SERIAL PRIMARY KEY, + geom GEOMETRY(PolygonZ, 4326) +); + +-- Insert example data into the tables +INSERT INTO public.point (geom) VALUES + (ST_GeomFromText('POINT Z(0 0 0)', 4326)), + (ST_GeomFromText('POINT Z(1 1 1)', 4326)), + (ST_GeomFromText('POINT Z(2 2 2)', 4326)), + (ST_GeomFromText('POINT Z(3 3 3)', 4326)), + (ST_GeomFromText('POINT Z(4 4 4)', 4326)); + +INSERT INTO public.linestring (geom) VALUES + (ST_GeomFromText('LINESTRING Z(0 0 0, 1 1 1)', 4326)), + (ST_GeomFromText('LINESTRING Z(2 2 2, 3 3 3)', 4326)), + (ST_GeomFromText('LINESTRING Z(4 4 4, 5 5 5)', 4326)), + (ST_GeomFromText('LINESTRING Z(6 6 6, 7 7 7)', 4326)), + (ST_GeomFromText('LINESTRING Z(8 8 8, 9 9 9)', 4326)); + +INSERT INTO public.polygon (geom) VALUES + (ST_GeomFromText('POLYGON Z((0 0 0, 1 0 0, 1 1 0, 0 1 0, 0 0 0))', 4326)), + (ST_GeomFromText('POLYGON Z((2 2 0, 3 2 0, 3 3 0, 2 3 0, 2 2 0))', 4326)), + (ST_GeomFromText('POLYGON Z((4 4 0, 5 4 0, 5 5 0, 4 5 0, 4 4 0))', 4326)), + (ST_GeomFromText('POLYGON Z((6 6 0, 7 6 0, 7 7 0, 6 7 0, 6 6 0))', 4326)), + (ST_GeomFromText('POLYGON Z((8 8 0, 9 8 0, 9 9 0, 8 9 0, 8 8 0))', 4326)); diff --git a/test/test_copyright.py b/test/test_copyright.py new file mode 100644 index 0000000..c968a9f --- /dev/null +++ b/test/test_copyright.py @@ -0,0 +1,30 @@ +# Copyright 2023 AIT - Austrian Institute of Technology GmbH +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +# @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + """Test source code for license compliance.""" + pkg_prefix = str(Path(__file__).parents[1]) + rci = main(argv=[pkg_prefix, 'test']) + assert rci == 0, 'Found errors' diff --git a/test/test_flake8.py b/test/test_flake8.py new file mode 100644 index 0000000..2f80434 --- /dev/null +++ b/test/test_flake8.py @@ -0,0 +1,30 @@ +# Copyright 2023 AIT - Austrian Institute of Technology GmbH +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + """Test source code for flake8 compliance.""" + pkg_prefix = str(Path(__file__).parents[1]) + rci, errors = main_with_errors(argv=[pkg_prefix]) + assert rci == 0, \ + f'Found {len(errors)} code style errors / warnings:\n' + \ + '\n'.join(errors) diff --git a/test/test_pep257.py b/test/test_pep257.py new file mode 100644 index 0000000..7cce2c3 --- /dev/null +++ b/test/test_pep257.py @@ -0,0 +1,28 @@ +# Copyright 2023 AIT - Austrian Institute of Technology GmbH +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path + +import pytest +from ament_pep257.main import main + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + """Test source code for PEP257 compliance.""" + pkg_prefix = str(Path(__file__).parents[1]) + rci = main(argv=[pkg_prefix, 'test']) + assert rci == 0, 'Found code style errors / warnings' diff --git a/test/test_postgis_converter.py b/test/test_postgis_converter.py new file mode 100644 index 0000000..a4ad6e5 --- /dev/null +++ b/test/test_postgis_converter.py @@ -0,0 +1,292 @@ +# Copyright 2023 AIT - Austrian Institute of Technology GmbH +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for PostGisConverter class.""" +import pytest +from scipy.spatial.transform import Rotation +from shapely import wkt +from std_msgs.msg import Header + +from postgis_ros_bridge.postgis_converter import PostGisConverter + +wkb_point_to_point = { + "POINT (10 20 30)": (10, 20, 30), + "POINT (0 0 0)": (0, 0, 0), + "POINT (1 2 3)": (1, 2, 3), + "POINT (1.1 2.2 3.3)": (1.1, 2.2, 3.3), + "POINT (1.1 2.2 3.3 4.4)": (1.1, 2.2, 3.3), + "POINT (1.1 2.2)": (1.1, 2.2, 0), + "POINT (-1.1 3.0 2.0)": (-1.1, 3.0, 2.0), + "POINT (1.1 -3.0 2.0)": (1.1, -3.0, 2.0), + "POINT (1.1 3.0 -2.0)": (1.1, 3.0, -2.0), + "POINT (-1.1 -3.0 -2.0)": (-1.1, -3.0, -2.0), +} + + +def test_to_point(): + """Test conversion from WKB to ROS Point.""" + for wkb_point, point in wkb_point_to_point.items(): + ros_point = PostGisConverter.to_point( + geometry=wkt.loads(wkb_point).wkb, as_hex=False) + assert ros_point.x == pytest.approx( + point[0]), f"check x for {wkb_point}" + assert ros_point.y == pytest.approx( + point[1]), f"check y for {wkb_point}" + assert ros_point.z == pytest.approx( + point[2]), f"check z for {wkb_point}" + + +def test_to_point_hex(): + """Test conversion from WKB to ROS Point.""" + for wkb_point, point in wkb_point_to_point.items(): + ros_point = PostGisConverter.to_point( + geometry=wkt.loads(wkb_point).wkb_hex, as_hex=True) + assert ros_point.x == pytest.approx( + point[0]), f"check x for {wkb_point}" + assert ros_point.y == pytest.approx( + point[1]), f"check y for {wkb_point}" + assert ros_point.z == pytest.approx( + point[2]), f"check z for {wkb_point}" + + for wkb_point, point in wkb_point_to_point.items(): + ros_point = PostGisConverter.to_point( + geometry=wkt.loads(wkb_point).wkb_hex) + assert ros_point.x == pytest.approx( + point[0]), f"check x for {wkb_point}" + assert ros_point.y == pytest.approx( + point[1]), f"check y for {wkb_point}" + assert ros_point.z == pytest.approx( + point[2]), f"check z for {wkb_point}" + + +def test_to_point_xyz(): + """Test conversion from WKB to tuple of x, y, z.""" + for wkb_point, point in wkb_point_to_point.items(): + point_tuple = PostGisConverter.to_point_xyz( + geometry=wkt.loads(wkb_point).wkb, as_hex=False) + assert point_tuple[0] == pytest.approx( + point[0]), f"check x for {wkb_point}" + assert point_tuple[1] == pytest.approx( + point[1]), f"check y for {wkb_point}" + if len(point_tuple) == 3: + assert point_tuple[2] == pytest.approx( + point[2]), f"check z for {wkb_point}" + + +def test_to_point_xyz_hex(): + """Test conversion from WKB to tuple of x, y, z.""" + for wkb_point, point in wkb_point_to_point.items(): + point_tuple = PostGisConverter.to_point_xyz( + geometry=wkt.loads(wkb_point).wkb_hex, as_hex=True) + assert point_tuple[0] == pytest.approx( + point[0]), f"check x for {wkb_point}" + assert point_tuple[1] == pytest.approx( + point[1]), f"check y for {wkb_point}" + if len(point_tuple) == 3: + assert point_tuple[2] == pytest.approx( + point[2]), f"check z for {wkb_point}" + for wkb_point, point in wkb_point_to_point.items(): + point_tuple = PostGisConverter.to_point_xyz( + geometry=wkt.loads(wkb_point).wkb_hex) + assert point_tuple[0] == pytest.approx( + point[0]), f"check x for {wkb_point}" + assert point_tuple[1] == pytest.approx( + point[1]), f"check y for {wkb_point}" + if len(point_tuple) == 3: + assert point_tuple[2] == pytest.approx( + point[2]), f"check z for {wkb_point}" + + +wkb_point_to_orientation = { + "POINT (0 0 0)": (Rotation.from_rotvec([0, 0, 0]).as_quat()), + "POINT (0 0 0.1)": (Rotation.from_rotvec([0, 0, 0.1]).as_quat()), + "POINT (0.1 0.2 0.3)": (Rotation.from_rotvec([0.1, 0.2, 0.3]).as_quat()), + "POINT (-0.1 0.2 0.3)": (Rotation.from_rotvec([-0.1, 0.2, 0.3]).as_quat()), +} + + +def test_to_orientation(): + """Test conversion from WKB to ROS Orientation.""" + for wkb_point, orientation in wkb_point_to_orientation.items(): + ros_orientation = PostGisConverter.to_orientation( + orientation=wkt.loads(wkb_point).wkb, as_hex=False) + assert ros_orientation.x == pytest.approx( + orientation[0]), f"check x for {wkb_point}" + assert ros_orientation.y == pytest.approx( + orientation[1]), f"check y for {wkb_point}" + assert ros_orientation.z == pytest.approx( + orientation[2]), f"check z for {wkb_point}" + assert ros_orientation.w == pytest.approx( + orientation[3]), f"check w for {wkb_point}" + + +def test_to_orientation_hex(): + """Test conversion from WKB to ROS Orientation.""" + for wkb_point, orientation in wkb_point_to_orientation.items(): + ros_orientation = PostGisConverter.to_orientation( + orientation=wkt.loads(wkb_point).wkb_hex, as_hex=True) + assert ros_orientation.x == pytest.approx( + orientation[0]), f"check x for {wkb_point}" + assert ros_orientation.y == pytest.approx( + orientation[1]), f"check y for {wkb_point}" + assert ros_orientation.z == pytest.approx( + orientation[2]), f"check z for {wkb_point}" + assert ros_orientation.w == pytest.approx( + orientation[3]), f"check w for {wkb_point}" + + for wkb_point, orientation in wkb_point_to_orientation.items(): + ros_orientation = PostGisConverter.to_orientation( + orientation=wkt.loads(wkb_point).wkb_hex) + assert ros_orientation.x == pytest.approx( + orientation[0]), f"check x for {wkb_point}" + assert ros_orientation.y == pytest.approx( + orientation[1]), f"check y for {wkb_point}" + assert ros_orientation.z == pytest.approx( + orientation[2]), f"check z for {wkb_point}" + assert ros_orientation.w == pytest.approx( + orientation[3]), f"check w for {wkb_point}" + + +wkb_pose_to_pose = { + ("POINT (10 20 30)", "POINT (0 0 0)"): + (10, 20, 30, *Rotation.from_rotvec([0, 0, 0]).as_quat()), + ("POINT (10 20 30)", "POINT (0 0 0.1)"): + (10, 20, 30, *Rotation.from_rotvec([0, 0, 0.1]).as_quat()), + ("POINT (10 20 30)", "POINT (0 0 0.1 0.1)"): + (10, 20, 30, *Rotation.from_rotvec([0, 0, 0.1]).as_quat()), + ("POINT (0 0 0)", "POINT (0 0.3 0.1)"): + (0, 0, 0, *Rotation.from_rotvec([0, 0.3, 0.1]).as_quat()), + ("POINT (-10 0 0)", "POINT (0 0.3 0.1)"): + (-10, 0, 0, *Rotation.from_rotvec([0, 0.3, 0.1]).as_quat()), +} + + +def test_to_pose(): + """Test conversion from WKB to ROS Pose.""" + for wkb_points, points in wkb_pose_to_pose.items(): + ros_pose = PostGisConverter.to_pose(geometry=wkt.loads( + wkb_points[0]).wkb, orientation=wkt.loads(wkb_points[1]).wkb, as_hex=False) + assert ros_pose.position.x == pytest.approx( + points[0]), f"check x for {wkb_points}" + assert ros_pose.position.y == pytest.approx( + points[1]), f"check y for {wkb_points}" + assert ros_pose.position.z == pytest.approx( + points[2]), f"check z for {wkb_points}" + assert ros_pose.orientation.x == pytest.approx( + points[3]), f"check x for {wkb_points}" + assert ros_pose.orientation.y == pytest.approx( + points[4]), f"check y for {wkb_points}" + assert ros_pose.orientation.z == pytest.approx( + points[5]), f"check z for {wkb_points}" + assert ros_pose.orientation.w == pytest.approx( + points[6]), f"check w for {wkb_points}" + + +def test_to_pose_hex(): + """Test conversion from WKB to ROS Pose.""" + for wkb_points, points in wkb_pose_to_pose.items(): + ros_pose = PostGisConverter.to_pose(geometry=wkt.loads( + wkb_points[0]).wkb_hex, orientation=wkt.loads(wkb_points[1]).wkb_hex, as_hex=True) + assert ros_pose.position.x == pytest.approx( + points[0]), f"check x for {wkb_points}" + assert ros_pose.position.y == pytest.approx( + points[1]), f"check y for {wkb_points}" + assert ros_pose.position.z == pytest.approx( + points[2]), f"check z for {wkb_points}" + assert ros_pose.orientation.x == pytest.approx( + points[3]), f"check x for {wkb_points}" + assert ros_pose.orientation.y == pytest.approx( + points[4]), f"check y for {wkb_points}" + assert ros_pose.orientation.z == pytest.approx( + points[5]), f"check z for {wkb_points}" + assert ros_pose.orientation.w == pytest.approx( + points[6]), f"check w for {wkb_points}" + + for wkb_points, points in wkb_pose_to_pose.items(): + ros_pose = PostGisConverter.to_pose(geometry=wkt.loads( + wkb_points[0]).wkb_hex, orientation=wkt.loads(wkb_points[1]).wkb_hex) + assert ros_pose.position.x == pytest.approx( + points[0]), f"check x for {wkb_points}" + assert ros_pose.position.y == pytest.approx( + points[1]), f"check y for {wkb_points}" + assert ros_pose.position.z == pytest.approx( + points[2]), f"check z for {wkb_points}" + assert ros_pose.orientation.x == pytest.approx( + points[3]), f"check x for {wkb_points}" + assert ros_pose.orientation.y == pytest.approx( + points[4]), f"check y for {wkb_points}" + assert ros_pose.orientation.z == pytest.approx( + points[5]), f"check z for {wkb_points}" + assert ros_pose.orientation.w == pytest.approx( + points[6]), f"check w for {wkb_points}" + + +def test_to_pose_stamped(): + """Test conversion from WKB to ROS PoseStamped.""" + for wkb_points, points in wkb_pose_to_pose.items(): + ros_pose = PostGisConverter.to_pose_stamped(header=Header(), geometry=wkt.loads( + wkb_points[0]).wkb, orientation=wkt.loads(wkb_points[1]).wkb, as_hex=False) + assert ros_pose.pose.position.x == pytest.approx( + points[0]), f"check x for {wkb_points}" + assert ros_pose.pose.position.y == pytest.approx( + points[1]), f"check y for {wkb_points}" + assert ros_pose.pose.position.z == pytest.approx( + points[2]), f"check z for {wkb_points}" + assert ros_pose.pose.orientation.x == pytest.approx( + points[3]), f"check x for {wkb_points}" + assert ros_pose.pose.orientation.y == pytest.approx( + points[4]), f"check y for {wkb_points}" + assert ros_pose.pose.orientation.z == pytest.approx( + points[5]), f"check z for {wkb_points}" + assert ros_pose.pose.orientation.w == pytest.approx( + points[6]), f"check w for {wkb_points}" + + +def test_to_pose_stamped_hex(): + """Test conversion from WKB to ROS PoseStamped.""" + for wkb_points, points in wkb_pose_to_pose.items(): + ros_pose = PostGisConverter.to_pose_stamped(header=Header(), geometry=wkt.loads( + wkb_points[0]).wkb_hex, orientation=wkt.loads(wkb_points[1]).wkb_hex, as_hex=True) + assert ros_pose.pose.position.x == pytest.approx( + points[0]), f"check x for {wkb_points}" + assert ros_pose.pose.position.y == pytest.approx( + points[1]), f"check y for {wkb_points}" + assert ros_pose.pose.position.z == pytest.approx( + points[2]), f"check z for {wkb_points}" + assert ros_pose.pose.orientation.x == pytest.approx( + points[3]), f"check x for {wkb_points}" + assert ros_pose.pose.orientation.y == pytest.approx( + points[4]), f"check y for {wkb_points}" + assert ros_pose.pose.orientation.z == pytest.approx( + points[5]), f"check z for {wkb_points}" + assert ros_pose.pose.orientation.w == pytest.approx( + points[6]), f"check w for {wkb_points}" + + for wkb_points, points in wkb_pose_to_pose.items(): + ros_pose = PostGisConverter.to_pose_stamped(header=Header(), geometry=wkt.loads( + wkb_points[0]).wkb_hex, orientation=wkt.loads(wkb_points[1]).wkb_hex) + assert ros_pose.pose.position.x == pytest.approx( + points[0]), f"check x for {wkb_points}" + assert ros_pose.pose.position.y == pytest.approx( + points[1]), f"check y for {wkb_points}" + assert ros_pose.pose.position.z == pytest.approx( + points[2]), f"check z for {wkb_points}" + assert ros_pose.pose.orientation.x == pytest.approx( + points[3]), f"check x for {wkb_points}" + assert ros_pose.pose.orientation.y == pytest.approx( + points[4]), f"check y for {wkb_points}" + assert ros_pose.pose.orientation.z == pytest.approx( + points[5]), f"check z for {wkb_points}" + assert ros_pose.pose.orientation.w == pytest.approx( + points[6]), f"check w for {wkb_points}" diff --git a/test/test_query_result_parser.py b/test/test_query_result_parser.py new file mode 100644 index 0000000..fb18140 --- /dev/null +++ b/test/test_query_result_parser.py @@ -0,0 +1,345 @@ +# Copyright 2023 AIT - Austrian Institute of Technology GmbH +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Test the query result parser.""" +import pytest +import pathlib +from builtin_interfaces.msg import Time +from rclpy.parameter import Parameter +from scipy.spatial.transform import Rotation +from sensor_msgs_py import point_cloud2 +from sqlalchemy import create_engine, text +from sqlalchemy.orm import sessionmaker +from sqlalchemy.pool import NullPool + +from postgis_ros_bridge.query_result_parser import ( + PC2ResultParser, PointResultParser, PolygonResultParser, + PolygonStampedResultParser, PoseResultParser, PoseStampedResultParser, + QueryResultDefaultParameters) + + +@pytest.fixture(name='testdir') +def get_testdir(tmpdir, request): + """Get test directory for the test module.""" + filename = request.module.__file__ + return pathlib.Path(filename).parent + + +@pytest.fixture(name='test_sql_files') +def get_test_sql_files(testdir): + """Get sql test file dict with paths.""" + sql_files = { + 'postgis_test': + testdir / 'sql_data' / 'postgis_test.sql', + } + return sql_files + + +@pytest.fixture(name='db_session') +def setup_db_session(postgresql): + """Create a database session for testing.""" + connection_uri = (f'postgresql+psycopg2://{postgresql.info.user}:' + f'@{postgresql.info.host}:' + f'{postgresql.info.port}/{postgresql.info.dbname}') + engine = create_engine(connection_uri, poolclass=NullPool) + session_ = sessionmaker(bind=engine) + session = session_() + yield session + session.close() + + +@pytest.fixture(name='db_session_test_db') +def setup_db_session_test_db(db_session, test_sql_files): + """Fill the database with test data.""" + for sql_file in test_sql_files.values(): + with open(file=sql_file, mode='r', encoding='latin-1') as file: + db_session.execute(text(file.read())) + db_session.commit() + return db_session + + +def test_simple_dummy_db_connection(db_session_test_db): + """Test if the dummy database connection works.""" + session = db_session_test_db + result = session.execute(text('SELECT * FROM point')) + assert result.rowcount == 5 + + +params = { + 'rate': Parameter(name='rate', value=1), + 'frame_id': Parameter(name='frame_id', value='test_frame_id'), + 'topic': Parameter(name='topic', value='test_topic'), + 'geodesic': Parameter(name='geodesic', value=False), +} + + +@pytest.fixture(name='query_result_default_parameters') +def setup_query_result_default_parameters(): + """Create a default parameter object.""" + defaults = QueryResultDefaultParameters() + defaults.declare_params() + defaults.set_params(params) + return defaults + + +@pytest.fixture(name='point_result_parser') +def setup_point_result_parser(query_result_default_parameters): + """Create a point result parser.""" + defaults = query_result_default_parameters + prp = PointResultParser() + prp.declare_params(defaults=defaults) + prp.set_params(params=params) + return prp + + +def test_point_result_parser(db_session_test_db, point_result_parser): + """Test if the point result parser works.""" + prp = point_result_parser + db = db_session_test_db + + assert prp.TYPE == 'PointStamped', 'check if type is set correctly' + + for element in db.execute(text("SELECT ROW_NUMBER() OVER (ORDER BY point.id) AS id, " + "point.geom AS geometry, 'test_frame_id' AS frame_id " + "FROM point")).all(): + res = prp.parse_single_element(element=element, time=Time()) + assert res[0], 'check if topic is set' + topic = res[0] + assert params['topic'].value == topic, 'check if topic is set correctly' + assert res[1], 'check if point is set' + point = res[1].point + assert point.x == pytest.approx( + element.id-1), 'check if point.x is set correctly' + assert point.y == pytest.approx( + element.id-1), 'check if point.y is set correctly' + assert point.z == pytest.approx( + element.id-1), 'check if point.z is set correctly' + + +@pytest.fixture(name='pose_result_parser') +def setup_pose_result_parser(query_result_default_parameters): + """Create a pose result parser.""" + defaults = query_result_default_parameters + prp = PoseResultParser() + prp.declare_params(defaults=defaults) + prp.set_params(params=params) + return prp + + +def test_pose_result_parser(db_session_test_db, pose_result_parser): + """Test if the pose result parser works.""" + prp = pose_result_parser + db = db_session_test_db + + assert prp.TYPE == 'Pose', 'check if type is set correctly' + + for element in db.execute(text("SELECT ROW_NUMBER() OVER (ORDER BY point.id) AS id, " + "point.geom AS geometry, point.geom AS rotation, " + "'test_frame_id' AS frame_id FROM point")).all(): + res = prp.parse_single_element(element=element, time=Time()) + assert res[0], 'check if topic is set' + topic = res[0] + assert params['topic'].value == topic, 'check if topic is set correctly' + assert res[1], 'check if point is set' + point = res[1].position + id = element.id-1 + assert point.x == pytest.approx( + id), 'check if point.x is set correctly' + assert point.y == pytest.approx( + id), 'check if point.y is set correctly' + assert point.z == pytest.approx( + id), 'check if point.z is set correctly' + rot = res[1].orientation + gt_rot = Rotation.from_rotvec([id, id, id]).as_quat() + assert rot.x == pytest.approx( + gt_rot[0]), 'check if rot.x is set correctly' + assert rot.y == pytest.approx( + gt_rot[1]), 'check if rot.y is set correctly' + assert rot.z == pytest.approx( + gt_rot[2]), 'check if rot.z is set correctly' + assert rot.w == pytest.approx( + gt_rot[3]), 'check if rot.w is set correctly' + + +@pytest.fixture(name='pose_stamped_result_parser') +def setup_pose_stamped_result_parser(query_result_default_parameters): + """Create a pose result parser.""" + defaults = query_result_default_parameters + prp = PoseStampedResultParser() + prp.declare_params(defaults=defaults) + prp.set_params(params=params) + return prp + + +def test_pose_stamped_result_parser(db_session_test_db, pose_stamped_result_parser): + """Test if the pose stamped result parser works.""" + prp = pose_stamped_result_parser + db = db_session_test_db + + assert prp.TYPE == 'PoseStamped', 'check if type is set correctly' + + for element in db.execute(text("SELECT ROW_NUMBER() OVER (ORDER BY point.id) AS id, " + "point.geom AS geometry, point.geom AS rotation, " + "'test_frame_id' AS frame_id FROM point")).all(): + res = prp.parse_single_element(element=element, time=Time()) + assert res[0], 'check if topic is set' + topic = res[0] + assert params['topic'].value == topic, 'check if topic is set correctly' + assert res[1], 'check if point is set' + point = res[1].pose.position + id = element.id-1 + assert point.x == pytest.approx( + id), 'check if point.x is set correctly' + assert point.y == pytest.approx( + id), 'check if point.y is set correctly' + assert point.z == pytest.approx( + id), 'check if point.z is set correctly' + rot = res[1].pose.orientation + gt_rot = Rotation.from_rotvec([id, id, id]).as_quat() + assert rot.x == pytest.approx( + gt_rot[0]), 'check if rot.x is set correctly' + assert rot.y == pytest.approx( + gt_rot[1]), 'check if rot.y is set correctly' + assert rot.z == pytest.approx( + gt_rot[2]), 'check if rot.z is set correctly' + assert rot.w == pytest.approx( + gt_rot[3]), 'check if rot.w is set correctly' + + +@pytest.fixture(name='pc2_result_parser') +def setup_pc2_result_parser(query_result_default_parameters): + """Create a pc2 result parser.""" + defaults = query_result_default_parameters + prp = PC2ResultParser() + prp.declare_params(defaults=defaults) + prp.set_params(params=params) + return prp + + +def test_pc2_result_parser(db_session_test_db, pc2_result_parser): + """Test if the pc2 result parser works.""" + prp = pc2_result_parser + db = db_session_test_db + + assert prp.TYPE == 'PointCloud2', 'check if type is set correctly' + + db_res = db.execute(text( + "SELECT ROW_NUMBER() OVER (ORDER BY point.id) AS id, point.geom AS geometry, " + "'test_frame_id' AS frame_id FROM point")) + res = prp.parse_result(result=db_res, time=Time()) + assert len(res) == 1, 'check if one result is returned' + res = res[0] + assert res[0], 'check if topic is set' + topic = res[0] + assert params['topic'].value == topic, 'check if topic is set correctly' + assert res[1], 'check if pc is set' + pc = res[1] + assert pc.height == 1, 'check if pc.height is set correctly' + assert pc.width == 5, 'check if pc.width is set correctly' + + pc_numpy = point_cloud2.read_points(pc, skip_nans=True) + for i, point in enumerate(pc_numpy): + assert point[0] == pytest.approx( + i), 'check if point.x is set correctly' + assert point[1] == pytest.approx( + i), 'check if point.y is set correctly' + assert point[2] == pytest.approx( + i), 'check if point.z is set correctly' + + +gt_polygons = [ + [[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 0]], + [[2, 2, 0], [3, 2, 0], [3, 3, 0], [2, 3, 0], [2, 2, 0]], + [[4, 4, 0], [5, 4, 0], [5, 5, 0], [4, 5, 0], [4, 4, 0]], + [[6, 6, 0], [7, 6, 0], [7, 7, 0], [6, 7, 0], [6, 6, 0]], + [[8, 8, 0], [9, 8, 0], [9, 9, 0], [8, 9, 0], [8, 8, 0]], +] + + +@pytest.fixture(name='polygon_result_parser') +def setup_polygon_result_parser(query_result_default_parameters): + """Create a polygon result parser.""" + defaults = query_result_default_parameters + prp = PolygonResultParser() + prp.declare_params(defaults=defaults) + prp.set_params(params=params) + return prp + + +def test_polygon_result_parser(db_session_test_db, polygon_result_parser): + """Test if the polygon result parser works.""" + prp = polygon_result_parser + db = db_session_test_db + + assert prp.TYPE == 'Polygon', 'check if type is set correctly' + db_polygons = db.execute(text( + "SELECT ROW_NUMBER() OVER (ORDER BY polygon.id) AS id, polygon.geom AS geometry, " + "'test_frame_id' AS frame_id FROM polygon")).all() + assert len(db_polygons) == len( + gt_polygons), 'check if all polygons are returned' + + for gt_polys, elements in zip(gt_polygons, db_polygons): + res = prp.parse_single_element(element=elements, time=Time()) + assert res[0], 'check if topic is set' + topic = res[0] + assert params['topic'].value == topic, 'check if topic is set correctly' + assert res[1], 'check if point is set' + points = res[1].points + assert len(points) == len(gt_polys), 'check if all points are returned' + for gt_poly, element in zip(gt_polys, points): + assert element.x == pytest.approx( + gt_poly[0]), 'check if point.x is set correctly' + assert element.y == pytest.approx( + gt_poly[1]), 'check if point.y is set correctly' + assert element.z == pytest.approx( + gt_poly[2]), 'check if point.z is set correctly' + + +@pytest.fixture(name='polygon_stamped_result_parser') +def setup_polygon_stamped_result_parser(query_result_default_parameters): + """Create a polygon stamped result parser.""" + defaults = query_result_default_parameters + prp = PolygonStampedResultParser() + prp.declare_params(defaults=defaults) + prp.set_params(params=params) + return prp + + +def test_polygon_stamped_result_parser(db_session_test_db, polygon_stamped_result_parser): + """Test if the polygon stamped result parser works.""" + prp = polygon_stamped_result_parser + db = db_session_test_db + + assert prp.TYPE == 'PolygonStamped', 'check if type is set correctly' + db_polygons = db.execute(text( + "SELECT ROW_NUMBER() OVER (ORDER BY polygon.id) AS id, " + "polygon.geom AS geometry, 'test_frame_id' AS frame_id FROM polygon")).all() + assert len(db_polygons) == len( + gt_polygons), 'check if all polygons are returned' + + for gt_polys, elements in zip(gt_polygons, db_polygons): + res = prp.parse_single_element(element=elements, time=Time()) + assert res[0], 'check if topic is set' + topic = res[0] + assert params['topic'].value == topic, 'check if topic is set correctly' + assert res[1], 'check if point is set' + points = res[1].polygon.points + assert len(points) == len(gt_polys), 'check if all points are returned' + for gt_poly, element in zip(gt_polys, points): + assert element.x == pytest.approx( + gt_poly[0]), 'check if point.x is set correctly' + assert element.y == pytest.approx( + gt_poly[1]), 'check if point.y is set correctly' + assert element.z == pytest.approx( + gt_poly[2]), 'check if point.z is set correctly'