diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..016ecb1 --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,39 @@ +ARG ROS_DISTRO=jazzy +FROM ros:${ROS_DISTRO} + +RUN apt update +RUN apt install -y \ + ros-${ROS_DISTRO}-navigation2 \ + ros-${ROS_DISTRO}-nav2-bringup \ + # ros-${ROS_DISTRO}-slam-toolbox \ + # ros-${ROS_DISTRO}-nav2-amcl \ + # ros-${ROS_DISTRO}-map-server \ + # ros-${ROS_DISTRO}-robot-localization \ + && rm -rf /var/lib/apt/lists/* + + +ENV COLCON_WS=/root/colcon_ws +ENV COLCON_WS_SRC=/root/colcon_ws/src + +COPY ./docker/entrypoint.bash /entrypoint.bash +RUN chmod +x /entrypoint.bash +ENTRYPOINT ["/entrypoint.bash"] + + + +COPY ./ ${COLCON_WS_SRC}/ricbot_navigation +COPY ./lib/mapdesc_ros ${COLCON_WS_SRC}/mapdesc_ros + +##### mapdesc installation - this should be done automatically with colcon if deps are defined correctly, but for some reason it is not +# RUN pip3 install -r ${COLCON_WS_SRC}/mapdesc_ros/mapdesc/requirements.txt + +# COPY ./lib/mapdesc_ros/mapdesc /opt/mapdesc +# WORKDIR /opt/mapdesc +# RUN pip3 install . +##### + +RUN cd ${COLCON_WS}\ + && . /opt/ros/${ROS_DISTRO}/setup.sh\ + && colcon build --packages-select ricbot_navigation mapdesc_msgs mapdesc_ros + +CMD ["ros2", "launch", "ricbot_navigation", "ricbot_nav.launch.py"] diff --git a/docker/entrypoint.bash b/docker/entrypoint.bash new file mode 100644 index 0000000..5e86be3 --- /dev/null +++ b/docker/entrypoint.bash @@ -0,0 +1,5 @@ +#!/bin/bash + +source /opt/ros/${ROS_DISTRO}/setup.sh +source ${COLCON_WS}/install/setup.sh +exec "$@" diff --git a/lib/mapdesc_ros/.devcontainer/devcontainer.json b/lib/mapdesc_ros/.devcontainer/devcontainer.json new file mode 100644 index 0000000..51999d6 --- /dev/null +++ b/lib/mapdesc_ros/.devcontainer/devcontainer.json @@ -0,0 +1,20 @@ +{ + "name": "MapDesc", + "workspaceFolder": "/root/colcon_ws", + "dockerComposeFile": "../compose.yml", + "overrideCommand": true, + "service": "mapdesc_ros_dev", + "customizations":{ + "vscode": { + "extensions": [ + "MermaidChart.vscode-mermaid-chart", + "ms-python.python", + "ms-python.debugpy", + "ms-python.flake8", + "ms-vscode.cpptools", + "ms-vscode.cpptools-extension-pack", + "twxs.cmake" + ] + } + } +} diff --git a/lib/mapdesc_ros/.gitignore b/lib/mapdesc_ros/.gitignore new file mode 100644 index 0000000..7a60b85 --- /dev/null +++ b/lib/mapdesc_ros/.gitignore @@ -0,0 +1,2 @@ +__pycache__/ +*.pyc diff --git a/lib/mapdesc_ros/.gitlab-ci.yml b/lib/mapdesc_ros/.gitlab-ci.yml new file mode 100644 index 0000000..39f79c5 --- /dev/null +++ b/lib/mapdesc_ros/.gitlab-ci.yml @@ -0,0 +1,34 @@ +image: d-reg.hb.dfki.de:5000/robot-config/ros-pip-pytest:humble-0.0.1 + +variables: + GIT_SUBMODULE_STRATEGY: recursive + +stages: + - build + - test + +mapdesc_ros build: + stage: build + script: + - cd mapdesc + - pip3 install -r requirements.txt + - pip3 install . + - cd .. + - colcon build + +mapdesc_ros test: + stage: test + script: + - cd mapdesc + - pip3 install -r requirements.txt + - pip3 install . + - cd .. + - colcon build + - source ./install/setup.bash + # Basic Unit Tests + - colcon test --pytest-with-coverage --event-handlers console_cohesion+ --return-code-on-test-failure + # Integration tests + - launch_test mapdesc_ros/integration_tests/launch_testing/marker_launch_test.py + artifacts: + paths: + - build/mapdesc_ros/ diff --git a/lib/mapdesc_ros/.gitmodules b/lib/mapdesc_ros/.gitmodules new file mode 100644 index 0000000..c006add --- /dev/null +++ b/lib/mapdesc_ros/.gitmodules @@ -0,0 +1,6 @@ +[submodule "mapdesc"] + path = mapdesc + url = ../mapdesc +[submodule "mapdesc_msgs"] + path = mapdesc_msgs + url = ../mapdesc_msgs diff --git a/lib/mapdesc_ros/Dockerfile b/lib/mapdesc_ros/Dockerfile new file mode 100644 index 0000000..fe8f2aa --- /dev/null +++ b/lib/mapdesc_ros/Dockerfile @@ -0,0 +1,21 @@ +ARG ROS_DISTRO +FROM d-reg.hb.dfki.de/robot-config/ros-pip-pytest:${ROS_DISTRO}-0.0.1 + +ENV COLCON_WS=/root/colcon_ws +ENV COLCON_WS_SRC=/root/colcon_ws/src + +COPY ./mapdesc/requirements.txt /tmp/requirements.txt +RUN pip3 install -r /tmp/requirements.txt && rm /tmp/requirements.txt + +COPY ./mapdesc /opt/mapdesc +WORKDIR /opt/mapdesc +RUN pip3 install . + +WORKDIR ${COLCON_WS} +COPY ./mapdesc_ros ${COLCON_WS_SRC}/mapdesc_ros +COPY ./mapdesc_msgs ${COLCON_WS_SRC}/mapdesc_msgs +RUN . /opt/ros/${ROS_DISTRO}/setup.sh && colcon build + +COPY ./run_tests.bash /run_tests.bash +RUN chmod +x /run_tests.bash +CMD [ "bash", "-c", "/run_tests.bash"] \ No newline at end of file diff --git a/lib/mapdesc_ros/Dockerfile-ros-pip-pytest b/lib/mapdesc_ros/Dockerfile-ros-pip-pytest new file mode 100644 index 0000000..5d29dcf --- /dev/null +++ b/lib/mapdesc_ros/Dockerfile-ros-pip-pytest @@ -0,0 +1,15 @@ +ARG ROS_DISTRO +FROM ros:${ROS_DISTRO} + +RUN apt-get update -qq \ + && apt-get install -y \ + python3-pip \ + python3-opencv \ + ros-${ROS_DISTRO}-std-msgs \ + ros-${ROS_DISTRO}-geometry-msgs \ + ros-${ROS_DISTRO}-diagnostic-msgs \ + ament-cmake \ + && rm -rf /var/lib/apt/lists/* + +# we need to upgrade flake8 to supress a deprecation warning. +RUN pip3 install --upgrade flake8 pytest-cov opencv-python diff --git a/lib/mapdesc_ros/Dockerfile_dev b/lib/mapdesc_ros/Dockerfile_dev new file mode 100644 index 0000000..5a0d414 --- /dev/null +++ b/lib/mapdesc_ros/Dockerfile_dev @@ -0,0 +1,24 @@ +ARG ROS_DISTRO +FROM d-reg.hb.dfki.de/robot-config/ros-pip-pytest:${ROS_DISTRO}-0.0.1 + +ENV COLCON_WS=/root/colcon_ws +ENV COLCON_WS_SRC=/root/colcon_ws/src + +COPY ./mapdesc/requirements.txt /tmp/requirements.txt +RUN pip3 install -r /tmp/requirements.txt && rm /tmp/requirements.txt + +COPY ./mapdesc /opt/mapdesc +WORKDIR /opt/mapdesc +# For development we want to overwrite mapdesc from local +RUN pip3 install -e . + +WORKDIR ${COLCON_WS} +COPY ./mapdesc_ros ${COLCON_WS_SRC}/mapdesc_ros +COPY ./mapdesc_msgs ${COLCON_WS_SRC}/mapdesc_msgs +# For development we want to overwrite mapdesc_ros and mapdesc_msgs from local +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && colcon build --symlink-install + +COPY ./run_tests.bash /run_tests.bash +RUN chmod +x /run_tests.bash +CMD [ "bash", "-c", "/run_tests.bash"] \ No newline at end of file diff --git a/lib/mapdesc_ros/README.md b/lib/mapdesc_ros/README.md new file mode 100644 index 0000000..b5e598f --- /dev/null +++ b/lib/mapdesc_ros/README.md @@ -0,0 +1,10 @@ +# MapDesc ROS 2 wrapper +A lightweight ROS 2 wrapper for [mapdesc](../mapdesc/) + +This wrapper has been generated using ROSCrud + +# Unit testing +see mapdesc_ros/test + +# Integration testing +see https://github.com/ros2/launch/tree/master/launch_testing/ diff --git a/lib/mapdesc_ros/compose.yml b/lib/mapdesc_ros/compose.yml new file mode 100644 index 0000000..016b147 --- /dev/null +++ b/lib/mapdesc_ros/compose.yml @@ -0,0 +1,31 @@ +services: + mapdesc_ros: + image: d-reg.hb.dfki.de/robot-config/mapdesc_ros:humble-0.0.1 + build: + context: . + dockerfile: Dockerfile + args: + ROS_DISTRO: humble + + mapdesc_ros_dev: + # custom Dockerfile that installs without moving files + # (so we can easily overwrite them from volumes for easier + # development with Dev Container) + build: + context: . + dockerfile: Dockerfile_dev + args: + ROS_DISTRO: humble + volumes: + - ./mapdesc:/opt/mapdesc:rw + - ./mapdesc_ros:/root/colcon_ws/src/mapdesc_ros:rw + - ./mapdesc_msgs:/root/colcon_ws/src/mapdesc_msgs:rw + - ./run_tests.bash:/root/colcon_ws/run_tests.bash:rw + + ros_with_pip: + image: d-reg.hb.dfki.de/robot-config/ros-pip-pytest:humble-0.0.1 + build: + context: . + dockerfile: Dockerfile-ros-pip-pytest + args: + ROS_DISTRO: humble \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/.coveragerc b/lib/mapdesc_ros/mapdesc/.coveragerc new file mode 100644 index 0000000..10f391c --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/.coveragerc @@ -0,0 +1,2 @@ +[run] +omit = mapdesc/cli.py \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/.devcontainer/devcontainer.json b/lib/mapdesc_ros/mapdesc/.devcontainer/devcontainer.json new file mode 100644 index 0000000..cee6b83 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/.devcontainer/devcontainer.json @@ -0,0 +1,17 @@ +{ + "name": "MapDesc", + "workspaceFolder": "/app/", + "dockerComposeFile": "../compose.yml", + "overrideCommand": true, + "service": "mapdesc", + "customizations":{ + "vscode": { + "extensions": [ + "MermaidChart.vscode-mermaid-chart", + "ms-python.python", + "ms-python.debugpy", + "ms-python.flake8" + ] + } + } +} diff --git a/lib/mapdesc_ros/mapdesc/.gitignore b/lib/mapdesc_ros/mapdesc/.gitignore new file mode 100644 index 0000000..89c2228 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/.gitignore @@ -0,0 +1,17 @@ +# python cache / pip +*.egg-info/ +__pycache__/ +*.pyc +build/ + +# py.test +.pytest_cache/ + +# test coverage +.coverage +htmlcov/ + +# cli test script +/generated/* +# OSM cache +cache/ \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/.gitlab-ci.yml b/lib/mapdesc_ros/mapdesc/.gitlab-ci.yml new file mode 100644 index 0000000..8b7f678 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/.gitlab-ci.yml @@ -0,0 +1,35 @@ +image: hdgigante/python-opencv:4.9.0-alpine + +stages: +- build +- test +- deploy + +mapdesc build: + stage: build + script: + - pip3 install . + +mapdesc test: + stage: test + script: + - pip3 install . + - pip3 install pytest coverage + - python3 -m pytest + - coverage run --source mapdesc --omit=mapdesc/cli.py -m pytest . + - coverage report -m + - coverage html + artifacts: + paths: + - htmlcov/ + +software_catalogue_entry: + image: d-reg.hb.dfki.de:5000/ubuntu:overview_generator + stage: deploy + script: + - apt update + - apt install -y wget + - wget http://bob.dfki.uni-bremen.de/software_overview/generate.sh + - sh generate.sh $CI_PROJECT_NAMESPACE $CI_PROJECT_NAME $CI_PROJECT_URL + only: + - main diff --git a/lib/mapdesc_ros/mapdesc/CONTRIBUTING.md b/lib/mapdesc_ros/mapdesc/CONTRIBUTING.md new file mode 100644 index 0000000..5b78ab5 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/CONTRIBUTING.md @@ -0,0 +1,54 @@ +# Contributing to *MapDesc* + +Please inform the maintainer as early as possible about your planned +feature developments, extensions, or bugfixes that you are working on. +An easy way is to open an issue or a pull request in which you explain +what you are trying to do. + +## Pull Requests + +The preferred way to contribute to *MapDesc* is to fork the main repository on Gitlab, then submit a "pull request" +(PR): + +1. Fork the [project repository](git@github.com:PROJECT_PATH): + click on the 'Fork' button near the top of the page. This creates a copy of + the code under your account on the Gitlab server. + +3. Clone this copy to your local disk: + + $ git clone git@github.com:YourLogin/MapDesc.git + +4. Create a branch to hold your changes: + + $ git checkout -b my-feature + + and start making changes. Never work in the ``main`` branch! + +5. Work on this copy, on your computer, using Git to do the version + control. When you're done editing, do:: + + $ git add modified_files + $ git commit + + to record your changes in Git, then push them to Gitlab with:: + + $ git push -u origin my-feature + +Finally, go to the web page of the your fork of the repo, +and click 'Pull request' to send your changes to the maintainers for review. + +## Merge Policy + +Summary: maintainer can push minor changes directly, pull request + 1 reviewer for everything else. + +* Usually it is not possible to push directly to the `main` branch of WBC for anyone. Only tiny changes, urgent bugfixes, and maintenance commits can be pushed directly to the `main` branch by the maintainer without a review. "Tiny" means backwards compatibility is mandatory and all tests must succeed. No new feature must be added. + +* Developers have to submit pull requests. Those will be reviewed by at least one other developer and merged by the maintainer. New features must be documented and tested. Breaking changes must be discussed and announced in advance with deprecation warnings. + +* Any change of existing functionality requires that all unit tests must succeed. In addition, the [tutorials](https://github.com/PROJECT_PATH/doc/tutorials) should be executed and the results should be compared with the results obtained prior to making those changes. If the results differ, the changes should be reconsidered. + +* Adding new functionality requires the addition of unit tests. In pinciple, every class should be accompanied by at least one unit test that checks the common use case, and one unit tests that checks for the common error cases + +## Project Roadmap + +Check the [Issue Tracker](https://github.com/PROJECT_PATH/issues) for roadmap planning. \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/LICENSE b/lib/mapdesc_ros/mapdesc/LICENSE new file mode 100644 index 0000000..0a0bfdf --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2025, DFKI GmbH, Andreas Bresser +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/lib/mapdesc_ros/mapdesc/README.md b/lib/mapdesc_ros/mapdesc/README.md new file mode 100644 index 0000000..d519383 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/README.md @@ -0,0 +1,112 @@ +# mapdesc + +**Map Desc**ription System for Robotics - generate and exports walls and other static objects from different sources to import into robotic simulations or as base for autonomous navigation. Can generate environments for navigation simulations (e.g. navsim-2d or Gazebo) from a given image as map using OpenCV or a map from a web-based editor. The map can also be exported into an image or YAML as input for the editor. + +MapDesc was initiated and is currently developed at the [Robotics Innovation Center](http://robotik.dfki-bremen.de/en/startpage.html) of the [German Research Center for Artificial Intelligence (DFKI)](http://www.dfki.de) in Bremen. + +![](doc/images/DFKI_RIC_RGB.jpg) + + +## Motivation +MapDesc allows you to import your data from different sources and export them into other formats + +### Inputs + +- OpenStreetMap (using OSMPythonTooly, you provide coordinates of a center point and a radius around it) +- ROS map (YAML and image file that is used by the [ROS map server](http://wiki.ros.org/map_server)) +- SDF-file environment description for gazebo (only parses collision box or mesh, ignores visual representation) +- YAML-description of walls, for example created by the [Map-Editor](../map-editor) (see format description) + +### Outputs +- SDF-files for gazebo simulation +- PNG-file (can be used as debug or to generate a PNG-file for the ROS map) +- SVG-file (can be used as debug to see the obstacle as individual objects) +- YAML-description of walls (see format description) that can be uploaded to the [Map-Editor](../map-editor) + + +![](doc/images/mapdesc_input_output.png) + +## Installation +**mapdesc** is written in python, so it can be installed for the current user using pip/pip3: + +```bash +# clone and cd into the mapdesc-directory +pip3 install . +``` + +### Dependencies +OpenCV2 is used for extracting and saving information from images. + +For Linux Ubuntu there are python-packages available: +```bash +sudo apt install python3-opencv +``` + +And for Linux Fedora: +```bash +sudo dnf install python3-opencv opencv +``` + +we also depend on these libraries: +- **pyyaml** to parse YAML-files like map descriptions and metadata. +- **jinja2** to generate SDF-files. +- **imutils** to get contours of an image when it gets loaded. +- **argcomplete** to autocomplete command-line arguments. + +## Getting Started +### Usage +see `mapdesc -h` + +Examples: + +```bash +# export rosmap yaml file and image to our own yaml-based format using opencv2 +mapdesc rosmap test/map/mallmap.yaml yaml test2.yml +# export a yaml-based description to an SDF file using jinja2 +mapdesc yaml test/yaml/simple_walls.yaml sdf test.sdf +``` + +### Troubleshooting +If you try to call `mapdesc` from your command line and you get the error message + +`mapdesc: command not found` + +you need to add your local bin-folder to your path, so just add + +`export PATH=~/.local/bin:$PATH` + +to your local `.bashrc` and source it again (`source ~/.bashrc`). + +## Format description + +The format is loosely based on the [SDFormat](http://sdformat.org/) geometry description to describe worlds in the robotic simulation [Gazebo](https://gazebosim.org/). See [SDF specification](http://sdformat.org/spec) for details. + +The format of the lanes-graph is loosely based on the ROS-messages for OpenRMF, see [RMF building map msgs](https://github.com/open-rmf/rmf_building_map_msgs/) for details. + +## Testing + +### Robotics / Gazebo +To test the generated SDF files with gazebo you can start gazebo with the SDF-folder as `GAZEBO_MODEL_PATH` variable, for example like this: `GAZEBO_MODEL_PATH=/home/user/mapdesc/generated/sdf/ gazebo` after you run the "`test_cli.bash`" file and the SDF-file you want to load is inside the generated-folder. + +Please check the unit tests [here](/test/). + +## Contributing + +Please use the [issue tracker](map_desc/issues) to submit bug reports and feature requests. Please use merge requests as described [here](/CONTRIBUTING.md) to add/adapt functionality. + +## License + +MapDesc is distributed under the [3-clause BSD license](https://opensource.org/licenses/BSD-3-Clause). + +## Maintainer / Authors / Contributers + +Andreas Bresser, andreas.bresser@dfki.de + +Copyright 2023, DFKI GmbH / Robotics Innovation Center diff --git a/lib/mapdesc_ros/mapdesc/clear_test.bash b/lib/mapdesc_ros/mapdesc/clear_test.bash new file mode 100755 index 0000000..c4952e5 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/clear_test.bash @@ -0,0 +1,9 @@ +#!/bin/bash +rm -Rf generated/sdf/test1/ +rm -Rf generated/sdf/test2/ +rm -f ./generated/mallmap.yml +rm -f generated/mallmap_bounding_box.yml +rm -f ./generated/test1.png +rm -f ./generated/test1.yml +rm -f ./generated/mallmap*.png +rm -f ./generated/hdp*.svg \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/compose.yml b/lib/mapdesc_ros/mapdesc/compose.yml new file mode 100644 index 0000000..439faf3 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/compose.yml @@ -0,0 +1,9 @@ +services: + mapdesc: + build: + context: ./ + dockerfile: ./docker/Dockerfile + environment: + - "PYTHONUNBUFFERED=1" + volumes: + - ./:/app/:rw \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/doc/images/DFKI_RIC_RGB.jpg b/lib/mapdesc_ros/mapdesc/doc/images/DFKI_RIC_RGB.jpg new file mode 100644 index 0000000..33e223e Binary files /dev/null and b/lib/mapdesc_ros/mapdesc/doc/images/DFKI_RIC_RGB.jpg differ diff --git a/lib/mapdesc_ros/mapdesc/doc/images/mallmap.png b/lib/mapdesc_ros/mapdesc/doc/images/mallmap.png new file mode 100644 index 0000000..1118382 Binary files /dev/null and b/lib/mapdesc_ros/mapdesc/doc/images/mallmap.png differ diff --git a/lib/mapdesc_ros/mapdesc/doc/images/mapdesc_input_output.png b/lib/mapdesc_ros/mapdesc/doc/images/mapdesc_input_output.png new file mode 100644 index 0000000..8da63a5 Binary files /dev/null and b/lib/mapdesc_ros/mapdesc/doc/images/mapdesc_input_output.png differ diff --git a/lib/mapdesc_ros/mapdesc/doc/images/mapdesc_input_output.svg b/lib/mapdesc_ros/mapdesc/doc/images/mapdesc_input_output.svg new file mode 100644 index 0000000..4c6a5a1 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/doc/images/mapdesc_input_output.svg @@ -0,0 +1,5356 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + YAML + + + + SVG + + + + PNG + + + + SDF + + + + mapdesc + + + + + + + + + + + + OpenStreetMap + + + + ROS map + + + + SDF + + + + YAML + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lib/mapdesc_ros/mapdesc/doc/images/world.png b/lib/mapdesc_ros/mapdesc/doc/images/world.png new file mode 100644 index 0000000..316e0ea Binary files /dev/null and b/lib/mapdesc_ros/mapdesc/doc/images/world.png differ diff --git a/lib/mapdesc_ros/mapdesc/doc/tutorials/01_basic_tutorial.md b/lib/mapdesc_ros/mapdesc/doc/tutorials/01_basic_tutorial.md new file mode 100644 index 0000000..5262000 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/doc/tutorials/01_basic_tutorial.md @@ -0,0 +1,53 @@ +# Basic tutorial +mapdesc provides a command line interface (cli) as well as a Python API. + +In this first tutorial we will look at the CLI, for the API take a look at [02: Basic API usage](02_basic_api_usage.md) + +## Bring a map to Gazebo simulation +mapdesc allows you convert data from one source to another. + +A typical use case would be to create an SDF-file form a recorded ROS map, so you could easily create a simulation environment of your recorded map for testing your robot in simulation first. + +First store your ROS 2 map using the [ROS map_server](http://wiki.ros.org/map_server) for ROS 1 or ROS 2. You can then load the map and export it to SDF. + +Replace `your_ros_map.yml` with the filename of the file from your map. +```bash +mapdesc rosmap your_ros_map.yml sdf ./generated/sdf/test2 +``` + +Then you could either copy the exported SDF to your `.gazebo/models`-folder or just run gazebo with the `GAZEBO_MODEL_PATH` set to the parent folder of where you generated the sdf-file: +```bash +GAZEBO_MODEL_PATH=/home/user/mapdesc/generated/sdf/ gazebo +``` +in Gazebo click on the "Insert"-tab. There you should see the model that you can insert in your gazebo world. + +## Load map to ROS map_server +1. Generate a ROSMap for a mapdesc description (or import it from another format like a YAML) +1. You can then use the YAML-file in your ROS 2 nav2 as part of the [map-server parameter](https://navigation.ros.org/configuration/packages/configuring-map-server.html#map-server-parameters) + + \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/doc/tutorials/02_basic_api_usage.md b/lib/mapdesc_ros/mapdesc/doc/tutorials/02_basic_api_usage.md new file mode 100644 index 0000000..5d7a5d7 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/doc/tutorials/02_basic_api_usage.md @@ -0,0 +1,91 @@ +# Examples + +1. Creating a map +2. Import and export data + +## Creating a map: + +For the beginning we will generate a simple map with two objects as `Marker` and an obstacle `Wall`. Let’s take a look at the mapdesc folder, for now the model and save folder are relevant. + + +`model` – contains the modules to describe the map, includes basic geometric descriptions for obstacles (for example box, cylinder) as well as their position and orientation (for example pose, vector2, quaternion...) +`save` – contains the modules to generate image files etc. from the model objects + +1. Import the required libraries: + + ```python + from mapdesc.model import Map, Marker, Wall + from mapdesc.model.geom import Pose, Vector3, Box, Dimenstion + from mapdesc import save + ``` + +2. Define what should be on the map, in this case 2 Marker named 'storage' and 'production' and a Wall named 'wall'. The map size is variable and will be set according to the position and dimensions of the outer objects. + + ```python + storage = Marker( + name = 'storage', + radius = 0.2, + pose = Pose( + position = Vector3(0,3,0) + ), + color = [255,0,0] + ) + + production = Marker( + name = 'production', + radius = 0.2, + pose = Pose( + position = Vector3(6,5,0) + ), + color = [0,0,255] + ) + wall = Wall(data = Box( + pose = Pose( + position = Vector3(3,5,0) + ), + size = Dimension(1.0,2.0,1.0) + ) + ) + ``` +3. Now we can generate the map that will contain the marker and the wall. + + ```python + world = Map( + name='world', + marker=[ + storage, production + ], + wall=[wall] + ) + ``` + +4. Almost done! In order to have an output we need to call the desired function from the save module. For example to export the whole map into a .png image. + ```python + save.save_png(world, 'world.png') + ``` + +5. The result will be saved as world.png below the folder where you run your code. It should look like this: +![](../images/world.png) + + +## Import and export data +In this example we will import a map recorded using the [ROS map_server](http://wiki.ros.org/map_server) and export it to an image file. +1. Import the required libraries: + ```python + from mapdesc.load.rosmap import load_rosmap + from mapdesc.save import save_png + ``` +2. Use the specific function to import the .yaml file + ```python + testmap = load_rosmap('./test/map/mallmap.yaml') + ``` + +3. Export it to .png. + ```python + save_png(testmap, './mallmap.png') + ``` +4. The output should like: + + ![](../images/mallmap.png) + +5. Be aware that the exported file may look slightly different to the imported one. Due to the fact that the import file is converted into its abstracted structure and then reconstructed to a file, so that some pixel information will not be identical, especially for circles or rotated objects we loose information. \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/doc/tutorials/03_import_path.md b/lib/mapdesc_ros/mapdesc/doc/tutorials/03_import_path.md new file mode 100644 index 0000000..016534d --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/doc/tutorials/03_import_path.md @@ -0,0 +1,35 @@ +# Import path from GeoJSON. + +For details on the GeoJSON format see https://www.rfc-editor.org/rfc/rfc7946 + +## Use Case #1: Create a path for a drone in a hilly area +### Create GeoJSON file +1. Go to https://geojson.io +1. Select the "Draw LineString"-tool on the right or press "l" +1. Create a path on the map +1. Click on "Save"/"GeoJSON" + + + +### Download and import from OSM +TODO: Describe how to Download and import + +### Alternative: Download from ??? +TODO: Describe how to Download GeoTIFF from NASA EarthData + +TODO: Convert/crop data using [geotiff_util](https://git.hb.dfki.de/samler-ki/simulation/geotiff_util/) + +### Import and manipulate data in MapEditor + +TODO: describe how to import the data into the MapEditor + +## Use Case #2: Create a path for a moon rover +### Create GeoJSON file + +1. Go to https://quickmap.lroc.asu.edu +1. Select the "Draw/Search tool" icon on the left +1. Click on the path "Arc" next to "Select Tool" +1. Create a path by clicking points on the map. Double-click when you are done. +1. Click on "Export Query Features"/Download Button (📥) in the center of the left "Draw & Query" tool. + +TODO: create mapdesc-file diff --git a/lib/mapdesc_ros/mapdesc/docker/Dockerfile b/lib/mapdesc_ros/mapdesc/docker/Dockerfile new file mode 100644 index 0000000..5983cc9 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/docker/Dockerfile @@ -0,0 +1,7 @@ +FROM hdgigante/python-opencv:4.9.0-alpine + +WORKDIR /app +COPY . /app/ + +RUN pip3 install pytest coverage +RUN pip3 install -e . \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/manifest.xml b/lib/mapdesc_ros/mapdesc/manifest.xml new file mode 100644 index 0000000..74d06cc --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/manifest.xml @@ -0,0 +1,38 @@ + + MapDesc + + **Map Desc**ription System for Robotics - generate and exports walls and other static objects from different sources to import into robotic simulations or as base for autonomous navigation. Can generate environments for navigation simulations (e.g. navsim-2d or Gazebo) from a given image as map using OpenCV or a map from a web-based editor. The map can also be exported into an image or yaml as input for the editor. + + Andreas Bresser/andreas.bresser@dfki.de + Andreas Bresser/andreas.bresser@dfki.de + + BSD-3-Clause + https://git.hb.dfki.de/phanes/mapdesc/ + + + + utilities + ROS1 + ROS2 + mapping + navigation + + + 0 + single project + + + active + + + + + python + ros + Development Status :: 3 - Alpha + + diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/__init__.py b/lib/mapdesc_ros/mapdesc/mapdesc/__init__.py new file mode 100644 index 0000000..82e286e --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/__init__.py @@ -0,0 +1,5 @@ +__version__ = '0.1' + +from . import model +from . import load +from . import save diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/cli.py b/lib/mapdesc_ros/mapdesc/mapdesc/cli.py new file mode 100644 index 0000000..3a9158b --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/cli.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 + +# PYTHON_ARGCOMPLETE_OK + +"""mapdesc command line tool.""" + +import argparse +import logging +import os +import sys + +from .load import LOAD +from .save import SAVE + +CONFIG_FORMATTER = '%(asctime)s %(name)s[%(levelname)s] %(message)s' +LOAD_ARGS = { + 'yaml': [('load_file', 'YAML filename (.yml/.yaml)')], + 'rosmap': [('load_file', 'ROS filename (.yml/.yaml), NOT the png')], + 'geojson': [ + ('load_file', 'Load GeoJSON file (.geojson)'), + ('planet', 'Planet (normally "earth")')], + 'sdf': [('load_file', 'SDF file (.xml/.sdf)')], + 'osm': [ + ('lat', 'latitude of coordinate'), + ('lon', 'longitude of coordinate'), + ('radius', 'radius around the given coordinate'), + ('planet', 'Planet (normally "earth")') + ] +} +SAVE_ARGS = { + 'png': [('file_name', 'PNG image (.png)')], + 'rosmap': [ + ( + 'file_name', + 'Name of YAML-File, will create a png from the map' + ) + ], + 'sdf': [ + ( + 'folder_name', + 'folder name of the model, will create a .sdf and config file' + ) + ], + 'svg': [('file_name', 'SVG file (.svg)')], + 'yaml': [ + ( + 'file_name', + 'Name of YAML-File, lossless, ' + 'based on the description with all information' + ) + ], +} + + +def setup_logging(): + log_level = os.environ.get('LOG_LEVEL', 'INFO') + log_level = getattr(logging, log_level) + logging.basicConfig(level=log_level, format=CONFIG_FORMATTER) + + +def print_help(): + print( + 'usage: mapdesc [-h] [LOAD_TYPE] [LOAD_PARAMS...] ' + '[SAVE_TYPE] [SAVE_PARAMS...]:') + print(__doc__) + print('') + print('LOAD_TYPE can be one of these (with LOAD_PARAMS):') + for key, arg in LOAD_ARGS.items(): + print(f' {key}: ') + for sarg in arg: + print(f' {sarg[0]}: {sarg[1]}') + print('') + print('SAVE_TYPE can be one of these (with SAVE_PARAMS):') + for key, arg in SAVE_ARGS.items(): + print(f' {key}: ') + for sarg in arg: + print(f' {sarg[0]}: {sarg[1]}') + print('') + print('examples:') + print('# convert yaml file to sdf') + print('mapdesc yaml test/yaml/simple_walls.yaml sdf ./generated/sdf/test1') + print('# get buildings from OSM and save as svg') + print('mapdesc osm 53.0762098 8.8075270 80 svg bremen_city.svg') + + +def main(): + setup_logging() + + parser = argparse.ArgumentParser(description=__doc__) + + parser.add_argument( + 'load_type', choices=LOAD_ARGS.keys(), + help='Type of loading operation') + parser.add_argument( + '--recenter', '-r', default=False, + action=argparse.BooleanOptionalAction + ) + parser.add_argument( + '--bounding_box', default=False, + action=argparse.BooleanOptionalAction + ) + # boxify creates a box from meshes that have 4 points as polygon that + # perfectly align as box + parser.add_argument( + '--boxify', '-b', default=False, + action=argparse.BooleanOptionalAction + ) + + parser.add_argument( + 'load_params', nargs='+', help='Parameters for loading operation') + + parser.add_argument( + 'save_type', choices=SAVE_ARGS.keys(), help='Type of saving operation') + parser.add_argument( + 'save_params', nargs='+', help='Parameters for saving operation') + + args = parser.parse_args() + + if len(LOAD_ARGS[args.load_type]) != len(args.load_params): + print( + f'Error: {args.load_type} operation requires ' + f'{len(LOAD_ARGS[args.load_type])} parameters ' + f'({len(args.load_params)} given)') + parser.print_help() + sys.exit(1) + + if len(SAVE_ARGS[args.save_type]) != len(args.save_params): + print( + f'Error: {args.save_type} operation requires ' + f'{len(SAVE_ARGS[args.save_type])} parameters ' + f'({len(args.save_params)} given)') + parser.print_help() + sys.exit(1) + + _map = LOAD[args.load_type](*args.load_params) + if not _map: + return + if args.recenter: + _map.recenter() + if args.bounding_box: + _map.bounding_box() + if args.boxify: + _map.boxify() + SAVE[args.save_type](_map, *args.save_params) + + +if __name__ == '__main__': + main() diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/data/README.md b/lib/mapdesc_ros/mapdesc/mapdesc/data/README.md new file mode 100644 index 0000000..b5cafd5 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/data/README.md @@ -0,0 +1,2 @@ +# ROS 2 template +Generate ROS 2 data. \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/data/templates/model.config.j2 b/lib/mapdesc_ros/mapdesc/mapdesc/data/templates/model.config.j2 new file mode 100644 index 0000000..1a35591 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/data/templates/model.config.j2 @@ -0,0 +1,15 @@ + + + {{ map.name }} + 1.0 + model.sdf + + + {{ author['name'] }} + {{ author['email'] }} + + + + {{ map.description }} + + diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/data/templates/model.sdf.j2 b/lib/mapdesc_ros/mapdesc/mapdesc/data/templates/model.sdf.j2 new file mode 100644 index 0000000..30b9474 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/data/templates/model.sdf.j2 @@ -0,0 +1,54 @@ + + + + 0 0 0 0 0 0 + {% for wall in map.walls %} + + + + {% if wall.type == 'box' -%} + + {{ wall.size.width }} {{ wall.size.length }} {{ wall.size.height }} + + {%- elif wall.type in ['polyline', 'mesh'] -%} + + {% for point in wall.points -%} + {{ point.x }} {{ point.y }} + {% endfor -%} + + {% endif %} + + 0 0 {{ wall.size.height / 2 }} 0 0 0 + + + + {% if wall.type == 'box' -%} + + {{ wall.size.width }} {{ wall.size.length }} {{ wall.size.height }} + + {%- elif wall.type == 'polyline' -%} + + {% for point in wall.points -%} + {{ point.x }} {{ point.y }} + {% endfor -%} + + {% endif %} + + 0 0 {{ wall.size.height / 2 }} 0 0 0 + + + 1 1 1 1 + + + 0 + + + {{ wall.pose.position.x }} {{ wall.pose.position.y }} {{ wall.pose.position.z }} {{ wall.pose.euler_orientation().x }} {{ wall.pose.euler_orientation().y }} {{ wall.pose.euler_orientation().z }} + + {% endfor %} + 1 + + diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/geo_data.py b/lib/mapdesc_ros/mapdesc/mapdesc/geo_data.py new file mode 100644 index 0000000..a61d0d8 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/geo_data.py @@ -0,0 +1,135 @@ +import math + + +PROJECTIONS = { + 'moon': + 'PROJCS["Equirectangular Moon",' + 'GEOGCS["GCS_Moon",DATUM["D_Moon",' + 'SPHEROID["Moon_localRadius",1737400,0]],' + 'PRIMEM["Reference_Meridian",0],' + 'UNIT["degree",0.0174532925199433,AUTHORITY["EPSG","9122"]]],' + 'PROJECTION["Equirectangular"],' + 'PARAMETER["standard_parallel_1",0],' + 'PARAMETER["central_meridian",0],' + 'PARAMETER["false_easting",0],' + 'PARAMETER["false_northing",0],' + 'UNIT["metre",1,AUTHORITY["EPSG","9001"]],' + 'AXIS["Easting",EAST],' + 'AXIS["Northing",NORTH]]' +} + +# Volumetric mean radius of planets and moons (approx. in meter) +RADIUS_IN_METERS = { + 'sun': 696342000, + + 'mercury': 2439700, + 'venus': 6051800, + 'earth': 6378137, + 'moon': 1737400, + 'mars': 3389500, + # Moons of Mars + 'phobos': 11266, + 'deimos': 6200, + + 'jupiter': 69911000, + # Moons of Jupiter + 'io': 1821600, + 'europa': 1560800, + 'ganymede': 2634100, + 'callisto': 2410300, + + 'saturn': 58232000, + # Moons of Saturn + 'mimas': 198200, + 'enceladus': 252100, + 'tethys': 531100, + 'dione': 561400, + 'rhea': 764300, + 'titan': 2574700, + 'iapetus': 734500, + + 'uranus': 25362000, + # Moons of Uranus + 'miranda': 235800, + 'ariel': 578900, + 'umbriel': 584700, + 'titania': 788400, + 'oberon': 761400, + + 'neptune': 24622000, + # Moon of Neptune + 'triton': 1353400, + + # dwarf planets + 'pluto': 1188300, + # Moons of pluto + 'charon': 606000, + 'nix': 24900, # diameter 49.8 × 33.2 × 31.1 + 'hydra': 25500, # diameter 50.9 × 36.1 × 30.9 + 'kerberos': 9500, # diameter 19 × 10 × 9 + 'styx': 8000, # diameter 16 × 9 × 8 + + 'eris': 1163000, + 'haumea': 780000, + 'makemake': 715000, + 'gongong': 615000, + 'quaora': 545000, + 'sedna': 500000, + 'ceres': 469700, + 'orcus': 435000, + 'vesta': 262700, + 'pallas': 255500, + 'hygiea': 216500, + 'juno': 135700, + 'chiron': 58350, + 'pholus': 49500, + 'nessus': 29000, +} + +SIGNS = { + 'sun': '☉', + 'mercury': '☿', + 'venus': '♀︎', + 'earth': '🜨', + 'moon': '☾', + 'mars': '♂︎', + 'jupiter': 'J', + + 'saturn': '♄', + 'uranus': '⛢', + + 'neptune': '♆', + + # dwarf planets + 'pluto': '♇', + 'eris': '⯰', + 'sedna': '⯲', + 'ceres': '⚳', + 'vesta': '⚶', + 'pallas': '⚴', + 'hygiea': '⯚', + 'juno': '⚵', + 'chiron': '⚷', + 'pholus': '⯛', + 'nessus': '⯜', +} + +CIRCUMFERENCE_IN_METERS = {} +for name, radius in RADIUS_IN_METERS.items(): + CIRCUMFERENCE_IN_METERS[name] = radius * math.pi * 2 + +METERS_PER_DEGREE_LATITUDE = {} +for name, circ in CIRCUMFERENCE_IN_METERS.items(): + METERS_PER_DEGREE_LATITUDE[name] = circ / 360 + + +def meters_per_degree_longitude(lat, body): + # A degree of longitude is widest at the equator at 111.321 km and + # gradually shrinks to zero at the poles. + return METERS_PER_DEGREE_LATITUDE[body] * math.cos(lat / 180 * math.pi) + + +def lon_lat_to_point(lat, lon, build_lat, buid_lon, body): + lonToMeter = meters_per_degree_longitude(build_lat, body) + latToMeter = METERS_PER_DEGREE_LATITUDE[body] + return (lon - buid_lon) * lonToMeter, (lat - build_lat) * latToMeter diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/load/__init__.py b/lib/mapdesc_ros/mapdesc/mapdesc/load/__init__.py new file mode 100644 index 0000000..03839ca --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/load/__init__.py @@ -0,0 +1,13 @@ +from .rosmap import load_rosmap +from .yaml import load_yaml +from .osm import load_osm +from .sdf import load_sdf +from .geojson import load_geojson + +LOAD = { + 'yaml': load_yaml, + 'rosmap': load_rosmap, + 'osm': load_osm, + 'sdf': load_sdf, + 'geojson': load_geojson +} diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/load/geojson.py b/lib/mapdesc_ros/mapdesc/mapdesc/load/geojson.py new file mode 100644 index 0000000..5af8f7c --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/load/geojson.py @@ -0,0 +1,54 @@ +from ..model import Map, Path +from pathlib import Path as PathLib +import json +import logging +from ..geo_data import lon_lat_to_point + +logger = logging.getLogger(__name__) + + +def parse_coordinates(first_lat, first_lon, coords, planet: str = 'earth'): + distances = [] + if not first_lat: + first_lon, first_lat = coords[0][0], coords[0][1] + coords = coords[1:] + for lon, lat in coords: + distances.append( + lon_lat_to_point(lat, lon, first_lat, first_lon, body=planet)) + return first_lon, first_lat, distances + + +def get_path_from_geojson(path, planet: str = 'earth'): + paths = [] + first_lat, first_lon = None, None + with open(str(path), encoding='utf-8') as fd: + data = json.load(fd) + for feature in data['features']: + if 'geometry' not in feature: + continue + if 'coordinates' not in feature['geometry']: + continue + if feature['geometry']['type'] == 'Point': + continue + if len(feature['geometry']['coordinates']) == 1: + continue + path = Path( + name='unknown path', + ) + paths.append(path) + first_lon, first_lat, coords = \ + parse_coordinates( + first_lat, first_lon, + feature['geometry']['coordinates'], + planet) + path.points = coords + return paths + + +def load_geojson(input_path=None, planet: str = 'earth'): + input_geojson = PathLib(input_path) + if not input_geojson.exists(): + raise RuntimeError('file/folder to load does not exist') + _map = Map() + _map.path = get_path_from_geojson(input_geojson, planet) + return _map diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/load/osm.py b/lib/mapdesc_ros/mapdesc/mapdesc/load/osm.py new file mode 100644 index 0000000..10b7eb3 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/load/osm.py @@ -0,0 +1,44 @@ +# load data description from OSM coordinages +from ..model import Map, Wall +from ..model.geom import Mesh +from OSMPythonTools.overpass import Overpass, overpassQueryBuilder +import numpy as np +from ..geo_data import lon_lat_to_point + + +def load_osm(lat: float, lon: float, radius: float, body: str = 'earth'): + lat = float(lat) + lon = float(lon) + radius = float(radius) + + north = lat + radius/111320 + south = lat - radius/111320 + east = lon + radius/111320/np.cos(lat*np.pi/180) + west = lon - radius/111320/np.cos(lat*np.pi/180) + + # Build the query using the OverpassQueryBuilder + query = overpassQueryBuilder( + bbox=(south, west, north, east), + elementType=['way', 'relation', 'node'], + selector='"building"="yes"', + out='body', + ) + + overpass = Overpass() + results = overpass.query(query).ways() + + # TODO: save buildings + # building_nodes = {} + + walls = [] + if results: + for way in results: + nodes = way.nodes() + points = [ + lon_lat_to_point( + lat, lon, n.lat(), n.lon(), body) for n in nodes] + mesh = Mesh(polygons=points) + wall = Wall(mesh, name=way.id) + walls.append(wall) + + return Map(wall=walls) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/load/rosmap.py b/lib/mapdesc_ros/mapdesc/mapdesc/load/rosmap.py new file mode 100644 index 0000000..395df82 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/load/rosmap.py @@ -0,0 +1,91 @@ +# load data description +import logging +import math +from pathlib import Path +import yaml +try: + import cv2 + from imutils import contours + import imutils + CV2_AVAILABLE = True +except ImportError: + CV2_AVAILABLE = False +from ..model import Map, Wall +from ..model.geom import Box, Mesh, Dimension, Vector2, Vector3 + +logger = logging.getLogger(__name__) + + +def _shapes_from_image(res, path): + """Generate list of x, y shapes from contours of an image + """ + if not CV2_AVAILABLE: + raise RuntimeError('Can not modify image, OpenCV2 not available') + img = cv2.imread(str(path)) + gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + edges = cv2.Canny(gray_img, 20, 100) + cnts = cv2.findContours( + edges.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) + cnts = imutils.grab_contours(cnts) + (cnts, _) = contours.sort_contours(cnts) + shapes = [] + for cnt in cnts: + # approx = cv2.approxPolyDP(cnt, 0.03*cv2.arcLength(cnt, True), True) + approx = cv2.approxPolyDP(cnt, 0.01*cv2.arcLength(cnt, True), True) + + coords = [(a[0][0], a[0][1]) for a in approx] + shapes.append(coords) + return shapes + + +def load_rosmap(yaml_file, height=2.0): + """Find contours in an image and create map from it. + """ + _map = Map() + _map.wall = [] + with open(yaml_file, encoding='utf-8') as f: + data = yaml.safe_load(f) + res = data['resolution'] + _map.origin.position = Vector3.from_any(data['origin']) + # get data from image + img_path = Path(yaml_file).absolute().parent / data['image'] + shapes = _shapes_from_image(res, img_path) + for coords in shapes: + points = [(x * res, y * res) for x, y in coords] + center = Mesh.calculate_position([ + Vector2(x, y) for x, y in points]) + if len(points) == 4 and \ + math.isclose( + math.dist(points[0], points[1]), + math.dist(points[2], points[3])) and \ + math.isclose( + math.dist(points[1], points[2]), + math.dist(points[3], points[0])): + + wall = Wall(data=Box()) + + center_top = (points[0][0] + points[1][0]), \ + (points[0][1] + points[1][1]) + center_btm = (points[2][0] + points[3][0]), \ + (points[2][1] + points[3][1]) + width = math.dist(center_top, center_btm) / 2 + + center_lft = (points[1][0] + points[2][0]), \ + (points[1][1] + points[2][1]) + center_rgt = (points[3][0] + points[0][0]), \ + (points[3][1] + points[0][1]) + length = math.dist(center_lft, center_rgt) / 2 + wall.data.size = Dimension(width, length, height) + else: + wall = Wall(data=Mesh()) + wall.data.polygons = [( + Vector2(*p))-center for p in points] + wall.data.size = Dimension(1.0, 1.0, height) + # our coordinates are relative (we subtracted center earlier so) + # we have to save it as the walls position + wall.data.pose.position = center + _map.wall.append(wall) + if not _map.wall: + logger.error('No walls found, aborting!') + return None + return _map diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/load/sdf.py b/lib/mapdesc_ros/mapdesc/mapdesc/load/sdf.py new file mode 100644 index 0000000..5c2b1c0 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/load/sdf.py @@ -0,0 +1,93 @@ +# load data description from SDL file +# see http://sdformat.org/tutorials +from ..model import Map, Wall +from ..model.geom import Box, Pose, Dimension, Vector3, \ + Quaternion, Mesh +from ..util import euler_to_quaternion +import logging +import xml.etree.ElementTree as ET +from pathlib import Path + +logger = logging.getLogger(__name__) + + +def parse_pose(pose: str): + pose = [float(x) for x in pose.split(' ')] + position = Vector3(*[float(x) for x in pose[0:3]]) + rotation = euler_to_quaternion(*pose[3:6]) + return Pose(position=position, orientation=Quaternion(*rotation)) + + +def parse_box_size(link_element): + size = link_element.find('collision/geometry/box/size').text.strip() + return Dimension(*[float(x) for x in size.split(' ')]) + + +def parse_link(link_element): + if link_element.find('collision/geometry/box'): + wall_type = 'box' + wall_data = Box( + pose=parse_pose(link_element.find('pose').text.strip()), + size=parse_box_size(link_element)) + elif link_element.find('collision/geometry/polyline'): + wall_type = 'polygon' + wall_data = Mesh( + polygons=[ + [float(y) for y in x.text.strip().split(' ')] for x in + link_element.findall('collision/geometry/polyline/point')]) + else: + raise RuntimeError('unknown xml geometry') + # we ignore the pose + return Wall( + name=link_element.attrib['name'], + data=wall_data, + type=wall_type, + ) + + +def get_walls_from_sdf(path): + walls = [] + with open(str(path), encoding='utf-8') as fd: + xml_string = fd.read() + root = ET.fromstring(xml_string) + model_element = root.find('model') + # we assume our model is static and + # everything is stored inside a link, + # we also ignore the initial pose of the model + # links are translated into walls + for link in model_element.findall('link'): + wall = parse_link(link) + if wall: + walls.append(wall) + return walls + + +def parse_sdf_dir(path): + with open(str(path / 'model.config'), encoding='utf-8') as fd: + xml_string = fd.read() + root = ET.fromstring(xml_string) + name_el = root.find('name') + desc_el = root.find('description') + sdf_el = root.find('sdf') + + description = desc_el.text.strip() if desc_el is not None else '' + name = name_el.text.strip() if name_el is not None else '' + sdf_file = sdf_el.text.strip() if sdf_el is not None else 'model.sdf' + + _map = Map(name=name, description=description) + _map.walls = get_walls_from_sdf(str(path / sdf_file)) + return _map + + +def load_sdf(input_path=None): + """Load from custom sdf format. + """ + input_sdf = Path(input_path) + if not input_sdf.exists(): + raise RuntimeError('file/folder to load does not exist') + if input_sdf.is_dir(): + _map = parse_sdf_dir(input_sdf) + elif str(input_path)[:4] in ['.sdf', '.xml']: + _map = Map() + _map.walls = get_walls_from_sdf(str(input_sdf)) + return _map diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/load/yaml.py b/lib/mapdesc_ros/mapdesc/mapdesc/load/yaml.py new file mode 100644 index 0000000..ad364e7 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/load/yaml.py @@ -0,0 +1,14 @@ +# load data description +from ..model import Map +import yaml +import logging + +logger = logging.getLogger(__name__) + + +def load_yaml(input_file=None): + """Load from custom yaml format. + """ + with open(input_file, encoding='utf-8') as fd: + yaml_data = yaml.safe_load(fd) + return Map(**yaml_data) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/__init__.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/__init__.py new file mode 100644 index 0000000..f628e32 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/__init__.py @@ -0,0 +1,22 @@ +# Data Description + +# In this file we allow unused imports. +# flake8: noqa + +from .path import Path + +# special description for maps +from .wall import Wall + +# roads/lanes +from .lane import BIDIRECTIONAL, UNIDIRECTIONAL +from .lane import LaneEdge, LaneGraph, LaneNode + +# regions for annotation or behaivor +from .area import Area + +# special points on the map to annotate for the user or for behavior +from .marker import Marker + +# map/graph +from .map import Map diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/area.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/area.py new file mode 100644 index 0000000..b22f597 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/area.py @@ -0,0 +1,80 @@ +# An area defines a region by a rectangle or polygon on the map, +# either as annotation for the user without function or +# for the planning system to change agent behavior when it enters a +# region. +# For example go to the next task when the agent moves +# into a goal area or change from a behavior where the robot +# follows a strict line to a behavior where the robot +# explores the map further. +from .geom.box import Box +from .geom.mesh import Mesh +from dataclasses import dataclass, field + + +TYPES = { + 'mesh': Mesh, + 'box': Box +} + + +@dataclass +class Area: + data: Box | Mesh + # geometric type (mesh or box) + type: str = 'mesh' + # custom type of your are, can for example be 'recharging' or 'unload' + # so other tools can use it to execute behavior based on this type + area_type: str = 'unnamed' + # name to identify area + name: str = None + # color as RGB array with values from 0 to 255 + color: list = field(default_factory=lambda: [50, 50, 255]) + + @property + def center(self): + return self.data.pose.position + + @property + def points(self): + return self.data.points + + @property + def size(self): + return self.data.size + + @property + def pose(self): + return self.data.pose + + def local_points(self): + return self.data.local_points() + + def __post_init__(self): + if isinstance(self.data, dict): + clz = TYPES[self.type] + # pylint: disable=not-a-mapping + self.data = clz(**self.data) + + def __iter__(self): + yield ('data', dict(self.data)) + yield ('type', self.type) + yield ('area_type', self.area_type) + if self.name: + yield ('name', self.name) + if self.color: + yield ('color', self.color) + + def copy(self): + return Area(**dict(self)) + + def bounding_box(self): + if self.type != 'mesh': + return + self.data = self.data.bounding_box() + self.type = 'box' + + def boxify(self): + if self.type != 'mesh': + return + self.data = self.data.boxify() + self.type = 'box' diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/ext.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/ext.py new file mode 100644 index 0000000..b2e456a --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/ext.py @@ -0,0 +1,19 @@ +# An external object, +from dataclasses import dataclass +# bounding box +from .geom.box import Box + + +@dataclass +class Ext: + name: str = None + type: str = 'gltf' # 'geotiff' | 'obj' | 'fbx' | 'ifc' | 'gltf' + data: Box + # list of strings + filenames: list + + def __iter__(self): + yield ('name', self.name) + yield ('type', self.type) + yield ('data', dict(self.data)) + yield ('filenames', list(self.filenames)) \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/__init__.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/__init__.py new file mode 100644 index 0000000..2d4759b --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/__init__.py @@ -0,0 +1,19 @@ +# In this file we allow unused imports. +# flake8: noqa + +# dots/points/vectors/nodes +from .vector2 import Vector2 # x, y +from .vector3 import Vector3 # x, y, z +from .quaternion import Quaternion # w, y, z, w +from .pose import Pose # Vector3 position and Quaternion orientation + +# physical description +from .dimension import Dimension + +# generic visual Objects +from .box import Box +from .plane import Plane +from .sphere import Sphere +from .capsule import Capsule +from .cylinder import Cylinder +from .mesh import Mesh # a 2D/3D list of points diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/box.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/box.py new file mode 100644 index 0000000..6725849 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/box.py @@ -0,0 +1,61 @@ +import dataclasses +import numpy as np + +from .vector2 import Vector2 +from .vector3 import Vector3 +from .dimension import Dimension +from .pose import Pose + + +@dataclasses.dataclass +class Box: + pose: Pose = dataclasses.field(default_factory=Pose) + size: Dimension = dataclasses.field(default_factory=Dimension) + + def __post_init__(self): + if isinstance(self.pose, dict): + # pylint: disable=not-a-mapping + self.pose = Pose(**self.pose) + if isinstance(self.size, dict): + # pylint: disable=not-a-mapping + self.size = Dimension(**self.size) + elif isinstance(self.size, (list, tuple, set)): + self.size = Dimension(*self.size) + + @property + def points(self): + # return points counter clockwise sorted points defining the outer + # hull in 2d + return [ + Vector2(-self.size.width/2, self.size.length/2), + Vector2(-self.size.width/2, -self.size.length/2), + Vector2(self.size.width/2, -self.size.length/2), + Vector2(self.size.width/2, self.size.length/2) + ] + + def local_points(self): + # apply pose (translate by position and rotate by orientation) + points = [] + # 1. rotate by orientation + # all points are centered/we rotate around x=0, y=0 + # returns a 3x3 matrix that we can multiply with our coordinates + matrix = self.pose.orientation.rotation_matrix() + for point in self.points: + if isinstance(point, Vector3): + point = list(point) + elif isinstance(point, Vector2): + point = list(point) + [0.0] + dot = np.dot(matrix, point) + points.append(Vector2(*dot[:2])) + # 2. translate by position (only x and y coordinates) + pose_2d = Vector3(self.pose.position.x, self.pose.position.y) + points = [p + pose_2d for p in points] + return points + + def __iter__(self): + yield ('pose', dict(self.pose)) + if self.size: + yield ('size', tuple(self.size)) + + def copy(self): + return Box(**dict(self)) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/capsule.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/capsule.py new file mode 100644 index 0000000..5225f6c --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/capsule.py @@ -0,0 +1,14 @@ +from dataclasses import dataclass + +from .box import Box + + +@dataclass +class Capsule(Box): + radius: float = 1.0 + length: float = 1.0 + + def __iter__(self): + yield from super().__iter__() + yield ('radius', self.radius) + yield ('length', self.length) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/cylinder.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/cylinder.py new file mode 100644 index 0000000..3a18c3d --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/cylinder.py @@ -0,0 +1,15 @@ +import dataclasses + + +from .box import Box + + +@dataclasses.dataclass +class Cylinder(Box): + radius: float = 1.0 + length: float = 1.0 + + def __iter__(self): + yield from super().__iter__() + yield ('radius', self.radius) + yield ('length', self.length) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/dimension.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/dimension.py new file mode 100644 index 0000000..8188c52 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/dimension.py @@ -0,0 +1,67 @@ +import dataclasses + + +@dataclasses.dataclass +class Dimension: + # Dimensions in meter, defaults to a 1x1x1 meter cube + width: float = 1.0 + length: float = 1.0 + height: float = 1.0 + + def __init__(self, width=1.0, length=1.0, height=1.0, *_, **__): + self.width = width + self.length = length + self.height = height + + def __iter__(self): + """ Helper to create a tuple from this """ + yield float(self.width) + yield float(self.length) + yield float(self.height) + + def __add__(self, o): + if isinstance(o, Dimension): + return Dimension( + self.width + o.width, + self.length + o.length, + self.height + o.height) + else: + return Dimension( + self.width + o, + self.length + o, + self.height + o) + + def __sub__(self, o): + if isinstance(o, Dimension): + return Dimension( + self.width - o.width, + self.length - o.length, + self.height - o.height) + else: + return Dimension( + self.width - o, + self.length - o, + self.height - o) + + def __mul__(self, o): + if isinstance(o, Dimension): + return Dimension( + self.width * o.width, + self.length * o.length, + self.height * o.height) + else: + return Dimension( + self.width * o, + self.length * o, + self.height * o) + + def __neg__(self): + return Dimension(-self.width, -self.length, -self.height) + + def null(self): + """check if all sides are 0.""" + return self.width == self.height == self.length == 0.0 + + def copy(self): + """create new Dimension instance with same data.""" + return Dimension(self.width, self.length, self.height) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/mesh.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/mesh.py new file mode 100644 index 0000000..b0c6c63 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/mesh.py @@ -0,0 +1,126 @@ +import math + +from dataclasses import dataclass, field + +from .box import Box +from .dimension import Dimension +from .vector2 import Vector2 +from .vector3 import Vector3 +from .quaternion import Quaternion +from ...util import ccw_sort, calculate_slope, \ + dot_product, euler_to_quaternion +from .pose import list_to_vector, Pose + + +@dataclass +class Mesh(Box): + polygons: list = field(default_factory=list) + + def __post_init__(self): + super().__post_init__() + self.polygons = [ + list_to_vector(poly) if isinstance(poly, (list, tuple)) else poly + for poly in self.polygons + ] + + def ccw_sort(self): + """sort polygon points counter-clockwise.""" + self.polygons = ccw_sort(self.polygons) + + @property + def points(self): + return self.polygons + + @staticmethod + def calculate_position(points, clz=Vector2): + # if you like to not use the zero-vector as position for the mesh + # but the center of it you can use this function to calculate the + # new position. Make sure to subtract the result of this vector + # from each point. + if clz == Vector2: + return Vector2( + sum([p.x for p in points]) / len(points), + sum([p.y for p in points]) / len(points)) + elif clz == Vector3: + return Vector3( + sum([p.x for p in points]) / len(points), + sum([p.y for p in points]) / len(points), + sum([p.z for p in points]) / len(points)) + else: + raise RuntimeError( + 'Only Vector2 or Vector3 are valid options as class, not ' + f'{clz}') + + def recenter(self): + clz = self.polygons[0].__class__ + center = Mesh.calculate_position(self.polygons, clz) + for idx, _ in enumerate(self.polygons): + self.polygons[idx] -= center + self.pose.position.x += center.x + self.pose.position.y += center.y + + def is_rectangle(self): + p = self.polygons + if len(p) != 4: + return False + slopes = [ + calculate_slope(p[0], p[1]), + calculate_slope(p[1], p[2]), + calculate_slope(p[2], p[3]), + calculate_slope(p[3], p[0]) + ] + if slopes[0] != slopes[2] or slopes[1] != slopes[3]: + return False + # check for right angle using dot product + if dot_product(p[0], p[1], p[2]) != 0: + return False + return True + + def boxify(self): + if not self.is_rectangle(): + # the mesh has to consist of exactly 4 polygons + return False + # recalculate center + + self.recenter() + + p = self.polygons + # calculate center of points right to the center + midpoint = p[0].midpoint(p[1]) + width = midpoint.distance(Vector2()) * 2 + length = p[1].midpoint(p[2]).distance(Vector2()) * 2 + + rot_z = math.atan2(midpoint.y, midpoint.x) + quat = euler_to_quaternion(0, 0, rot_z) + self.pose.orientation = Quaternion(*quat) + + return Box(size=Dimension(width, length), pose=self.pose) + + def bounding_box(self): + """create a new bounding box around the points.""" + min_x = min([p.x for p in self.polygons]) + max_x = max([p.x for p in self.polygons]) + min_y = min([p.y for p in self.polygons]) + max_y = max([p.y for p in self.polygons]) + + size = Dimension() + size.width = float(max_x - min_x) + + pose = Pose(orientation=self.pose.orientation.copy()) + pose.position.x = float(min_x + max_x) / 2 + + pose.position.y = float(min_y + max_y) / 2 + pose.position.z = float(pose.position.z) + size.length = float(max_y - min_y) + + if isinstance(self.polygons[0], Vector3): + min_z = min([p.z for p in self.polygons]) + max_z = max([p.z for p in self.polygons]) + pose.position.z = float(min_z + max_z) / 2 + size.height = float(max_z - min_z) + + return Box(pose=pose, size=size) + + def __iter__(self): + yield from super().__iter__() + yield ('polygons', [list(p) for p in self.polygons]) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/plane.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/plane.py new file mode 100644 index 0000000..b39ef56 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/plane.py @@ -0,0 +1,23 @@ +from dataclasses import dataclass, field + +from .vector3 import Vector3 +from .box import Box + + +# TODO: let the Box inerhit plane - a box is a plane with a height +# or have them separated? +@dataclass +class Plane(Box): + normal: Vector3 = field(default_factory=Vector3()) + + def __post_init__(self): + super().__post_init__() + if isinstance(self.normal, (list, tuple)): + self.normal = Vector3(*self.normal) + # default for dimension is a 1x1x1 cube, so we have to set it to 0. + self.size.height = 0.0 + + def __iter__(self): + yield from super().__iter__() + yield ('normal', tuple(self.normal)) + yield ('size', tuple(self.size)[0:2]) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/pose.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/pose.py new file mode 100644 index 0000000..b0d1411 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/pose.py @@ -0,0 +1,80 @@ +from dataclasses import dataclass, field +from .quaternion import Quaternion +from .vector2 import Vector2 +from .vector3 import Vector3 + + +def list_to_vector(data): + """Creates a Vector or Quaternion from a list by the list size.""" + if len(data) == 2: + return Vector2(*data) + if len(data) == 3: + return Vector3(*data) + if len(data) == 4: + return Quaternion(*data) + raise RuntimeError(f'data for vector should be of length 2-4: {data}') + + +def dict_to_vector(data): + """Creates a Vector or Quaternion from a dict. + + Needs at least x and y for a 2d pose, creates a 3D-vector for x, y, z + and a Quaternion if w is also given. + """ + if 'x' not in data or 'y' not in data: + raise RuntimeError('x and y not set, not a valid pose.') + if 'w' in data: + return Quaternion(**data) + if 'z' in data: + return Vector3(**data) + return Vector2(**data) + + +def any_to_vector(data): + """converts any data to a vector or Quaternion if possible. + + see @dict_to_vector and @list_to_vector. + """ + if isinstance(data, (list, tuple, set)): + return list_to_vector(data) + if isinstance(data, dict): + return dict_to_vector(data) + if isinstance(data, (Vector2, Vector3, Quaternion)): + return data + raise RuntimeError(f'unknown vector type {type(data)} ({data})') + + +@dataclass +class Pose: + """Describes the pose of an object. + + (defined by its position and orientation).""" + position: Vector3 = field(default_factory=Vector3) + orientation: Quaternion = field(default_factory=Quaternion) + _euler_orientation: Vector3 = field(default_factory=Vector3) + + def __post_init__(self): + if self.orientation: + self.orientation = any_to_vector(self.orientation) + if self.position: + self.position = any_to_vector(self.position) + + def euler_orientation(self): + """Create euler orientation from Quaternion. + + Caches the generated euler orientation to a local _euler_orientation. + Note that it might be different from the given orientation-quaternion. + """ + if isinstance(self.orientation, Quaternion): + self._euler_orientation = self.orientation.to_euler() + elif isinstance(self.orientation, Vector3): + self._euler_orientation = self.orientation + return self._euler_orientation + + def __iter__(self): + yield ('orientation', tuple(self.orientation)) + yield ('position', tuple(self.position)) + + def copy(self): + """copy values from current Pose to a new Pose-instance.""" + return Pose(**dict(self)) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/quaternion.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/quaternion.py new file mode 100644 index 0000000..5dacfcd --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/quaternion.py @@ -0,0 +1,103 @@ +from dataclasses import dataclass + +from ...util import quaternion_to_euler +from .vector3 import Vector3 + + +@dataclass +class Quaternion(Vector3): + # a quaternion describes rotation in radians. + w: float = 1.0 + + def __init__(self, x=0.0, y=0.0, z=0.0, w=1.0, *args, **kwargs): + super().__init__(x, y, z) + self.w = w + + def __iter__(self): + """ Helper to create a tuple from this """ + yield self.x + yield self.y + yield self.z + yield self.w + + def __getitem__(self, idx): + if idx in [3, 'w']: + return self.w + return super().__getitem__(idx) + + def __add__(self, o): + if isinstance(o, Quaternion): + return Quaternion( + self.x + o.x, self.y + o.y, self.z + o.z, self.w + o.w) + else: + return Quaternion(*[val + o for val in self]) + + def __sub__(self, o): + if isinstance(o, Quaternion): + return Quaternion( + self.x - o.x, self.y - o.y, self.z - o.z, self.w - o.w) + else: + return Quaternion(*[val - o for val in self]) + + def __mul__(self, o): + """ Scalar multiplication """ + if isinstance(o, Quaternion): + return Quaternion( + self.x * o.x, self.y * o.y, self.z * o.z, self.w * o.w) + else: + return Quaternion(*[val * o for val in self]) + + def __neg__(self): + """ Returns a vector pointing in the opposite direction """ + return Quaternion(*[-val for val in self]) + + def normalize(self): + """ Normalizes the vector to have unit length """ + mag = self.magnitude() + if not mag: + raise RuntimeError( + f'Can not normalize null quaternion {self}') + return Quaternion(*[q / mag for q in self]) + + def copy(self): + return Quaternion(self.x, self.y, self.z, self.w) + + @staticmethod + def from_any(lst): + if isinstance(lst, Quaternion): + return lst + if isinstance(lst, (list, tuple)): + return Quaternion(*lst[:4]) + elif isinstance(lst, dict): + return Quaternion(**lst) + else: + raise RuntimeError('given data neither list nor dict') + + def rotation_matrix(self) -> list: + # see https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation# + # Conversion_to_and_from_the_matrix_representation + + # get a normalized copy of the quaternion + quat = self.normalize() + + # Calculate the elements of the rotation matrix + w, x, y, z = quat.w, quat.x, quat.y, quat.z + xx = x * x + xy = x * y + xz = x * z + yy = y * y + yz = y * z + zz = z * z + wx = w * x + wy = w * y + wz = w * z + + # Construct and return the rotation matrix + return [[1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy)], + [2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx)], + [2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy)]] + + def to_euler(self): + e = Vector3() + e.x, e.y, e.z = quaternion_to_euler(self.x, self.y, self.z, self.w) + return e diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/sphere.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/sphere.py new file mode 100644 index 0000000..1241ac9 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/sphere.py @@ -0,0 +1,12 @@ +from dataclasses import dataclass + +from .box import Box + + +@dataclass +class Sphere(Box): + radius: float = 1.0 + + def __iter__(self): + yield from super().__iter__() + yield ('radius', self.radius) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/vector2.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/vector2.py new file mode 100644 index 0000000..2f9a190 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/vector2.py @@ -0,0 +1,115 @@ +import dataclasses +import math + + +@dataclasses.dataclass +class Vector2: + x: float = 0.0 + y: float = 0.0 + + def __init__(self, x=0.0, y=0.0, *args, **kwargs): + self.x = x + self.y = y + + def __getitem__(self, idx): + if idx in [0, 'x']: + return self.x + elif idx in [1, 'y']: + return self.y + elif idx in [2, 'z']: + # just in case someone wanted a vector3 instead + return 0.0 + else: + raise RuntimeError(f'unknown key "{idx}"') + + def serialize(self): + """Serializes the vector as tuple.""" + return self.to_tuple() + + def to_tuple(self): + """generates a tuple using the dataclasses.astuple helper function.""" + return tuple(float(i) for i in dataclasses.astuple(self)) + + def __iter__(self): + """The default iteration for vectors behaves like a list or tuple.""" + yield float(self.x) + yield float(self.y) + + def __add__(self, o): + if isinstance(o, Vector2): + return Vector2(self.x + o.x, self.y + o.y) + else: + return Vector2(*[val + o for val in self]) + + def __round__(self, num): + return Vector2( + round(self.x, num), + round(self.y, num)) + + def __sub__(self, o): + if isinstance(o, Vector2): + return Vector2(self.x - o.x, self.y - o.y) + else: + return Vector2(*[val - o for val in self]) + + def __mul__(self, o): + """ Scalar multiplication """ + if isinstance(o, Vector2): + return Vector2(self.x * o.x, self.y * o.y) + else: + return Vector2(*[val * o for val in self]) + + def __neg__(self): + """ Returns the vector pointing in the opposite direction """ + return Vector2(-self.x, -self.y) + + def magnitude(self): + """ Calculate the euclidean length of the vector """ + return math.sqrt(sum([val**2 for val in self])) + + def normalize(self): + """ Normalizes the vector to have unit length """ + return Vector2(*[q / self.magnitude() for q in self]) + + def null(self): + """Check if both, x and y are 0.""" + return self.x == self.y == 0 + + def copy(self): + """create a new vector2 instance.""" + return Vector2(*self) + + @staticmethod + def from_any(_any): + if isinstance(_any, (Vector2)): + return _any + if isinstance(_any, (list, tuple, set)): + return Vector2(*_any) + if isinstance(_any, dict): + return Vector2(**_any) + raise RuntimeError('given data neither list nor dict') + + def is_close(self, other, rel_tol=0.01, abs_tol=0.01, threshold=None): + """ Check if a Vector2 is close to another Vector2. """ + if not other: + return False + if isinstance(other, dict): + x, y = other['x'], other['y'] + elif isinstance(other, (list, tuple, set)): + x, y = other + else: + x, y = other.x, other.y + if threshold: + return abs(self.x - x) + abs(self.y + y) < threshold + return math.isclose( + self.x, x, rel_tol=rel_tol, abs_tol=abs_tol) and \ + math.isclose( + self.y, y, rel_tol=rel_tol, abs_tol=abs_tol) + + def distance(self, other): + """ Calculate the distance to the given position """ + return (self - other).magnitude() + + def midpoint(self, other): + """Return a midpoint between the two points.""" + return Vector2((self.x + other.x) / 2, (self.y + other.y) / 2) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/vector3.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/vector3.py new file mode 100644 index 0000000..d9bfb43 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/geom/vector3.py @@ -0,0 +1,96 @@ +import dataclasses +import math +from .vector2 import Vector2 + + +@dataclasses.dataclass +class Vector3(Vector2): + z: float = 0.0 + + def __init__(self, x=0.0, y=0.0, z=0.0, *args, **kwargs): + super().__init__(x, y) + self.z = z + + def __getitem__(self, idx): + if idx in [2, 'z']: + return self.z + return super().__getitem__(idx) + + def __iter__(self): + """ Helper to create a tuple from this """ + yield self.x + yield self.y + yield self.z + + def __add__(self, o): + if isinstance(o, Vector3): + return Vector3(self.x + o.x, self.y + o.y, self.z + o.z) + else: + return Vector3(*[val + o for val in self]) + + def __sub__(self, o): + if isinstance(o, Vector3): + return Vector3(self.x - o.x, self.y - o.y, self.z - o.z) + else: + return Vector3(*[val - o for val in self]) + + def __mul__(self, o): + """ Scalar multiplication """ + if isinstance(o, Vector3): + return Vector3(self.x * o.x, self.y * o.y, self.z * o.z) + else: + return Vector3(*[val * o for val in self]) + + def __neg__(self): + """ Returns a vector pointing in the opposite direction """ + return Vector3(*[-val for val in self]) + + def __round__(self, num): + return Vector3( + round(self.x, num), + round(self.y, num), + round(self.z, num)) + + def normalize(self): + """ Normalizes the vector to have unit length """ + return Vector3(*[q / self.magnitude() for q in self]) + + def copy(self): + return Vector3(self.x, self.y, self.y) + + def is_close(self, o, rel_tol=0.01, abs_tol=0.01, threshold=None): + """ Check if a Vector3 is close to another Vector3. """ + if not o: + return False + if isinstance(o, dict): + x, y, z = o['x'], o['y'], o['z'] + elif isinstance(o, (list, tuple, set)): + x, y, z = o + else: + x, y, z = o.x, o.y, o.z + if threshold: + return abs(self.x - x) + abs(self.y - y) + abs(self.z - z) < \ + threshold + return math.isclose( + self.x, x, rel_tol=rel_tol, abs_tol=abs_tol) and \ + math.isclose( + self.y, y, rel_tol=rel_tol, abs_tol=abs_tol) and \ + math.isclose( + self.z, z, rel_tol=rel_tol, abs_tol=abs_tol) + + @staticmethod + def from_any(lst): + if isinstance(lst, Vector3): + return lst + if isinstance(lst, (list, tuple, set)): + return Vector3(*lst) + if isinstance(lst, dict): + return Vector3(**lst) + raise RuntimeError('given data neither list nor dict') + + def midpoint(self, other): + """Return a midpoint between the two points.""" + return Vector3( + (self.x + other.x) / 2, + (self.y + other.y) / 2, + (self.z + other.z) / 2) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/lane.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/lane.py new file mode 100644 index 0000000..77d0a6f --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/lane.py @@ -0,0 +1,100 @@ +# lane inspired by +# https://github.com/open-rmf/rmf_building_map_msgs/ +from dataclasses import dataclass, field +from .geom.vector3 import Vector3 + + +BIDIRECTIONAL = 0 +UNIDIRECTIONAL = 1 + + +@dataclass +class LaneGraph: + nodes: list = field(default_factory=list) # list of LaneNode + edges: list = field(default_factory=list) # list of edges + _last_new_number: int = 0 # used to optimize generation of unique ids + + def _unique_node_name(self): + """Generate a new unique name for a node.""" + known_names = [n.name for n in self.nodes] + while True: + new_name = f'node_{self._last_new_number}' + if new_name in known_names: + self._last_new_number += 1 + else: + return new_name + + def __post_init__(self): + self.nodes = [ + LaneNode(**n) if isinstance(n, dict) else n + for n in self.nodes + ] + self.edges = [ + LaneEdge(**e) if isinstance(e, dict) else e + for e in self.edges + ] + # generate unique id for all nodes if necessary + for node in self.nodes: + if not node.name: + node.name = self._unique_node_name() + for edge in self.edges: + edge.graph = self + edge.update_nodes() + + def __iter__(self): + yield ('nodes', [dict(node) for node in self.nodes]) + yield ('edges', [dict(edge) for edge in self.edges]) + + +@dataclass +class LaneNode: + # node in the navigation graph (similar to marker.Marker) + position: Vector3 = field(default_factory=Vector3) + name: str = None + # additional information about this node that the user can define + # as key-value pair + params: dict = field(default_factory=dict) + + def __post_init__(self): + self.position = Vector3.from_any(self.position) + + def __iter__(self): + yield ('name', self.name) + yield ('position', list(self.position)) + if self.params: + yield ('params', self.params) + + +@dataclass +class LaneEdge: + source: LaneNode = None + target: LaneNode = None + graph: LaneGraph = None + + # Unidirectional lanes only allow driving from source to target + # not from target to source + edge_type: int = BIDIRECTIONAL # enum UNIDIRECTIONAL or BIDIRECTIONAL + + # optional name for the edge + name: str = None + + # additional information about this edge that the user can define + params: dict = None + + def update_nodes(self): + """Set source and target nodes based on their poisitons + in the graph. + """ + if isinstance(self.source, int): + self.source = self.graph.nodes[self.source] + if isinstance(self.target, int): + self.target = self.graph.nodes[self.target] + + def __iter__(self): + yield ('source', self.graph.nodes.index(self.source)) + yield ('target', self.graph.nodes.index(self.target)) + if self.name: + yield ('name', self.name) + if self.params: + yield ('params', self.params) + yield ('edge_type', self.edge_type) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/map.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/map.py new file mode 100644 index 0000000..61619b0 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/map.py @@ -0,0 +1,117 @@ +from dataclasses import dataclass, field + +from .geom.dimension import Dimension +from .geom.vector3 import Vector3 +from .wall import Wall +from .marker import Marker +from .area import Area +from .lane import LaneGraph +from .path import Path + + +@dataclass +class Map: + # name has to be unique as its also the file-name + name: str = 'undefined' + # optional description of the map + description: str = 'some map' + + # size of the map + size: Dimension = field(default_factory=Dimension) + # resolution - important for export for the ROS 2 mapserver + resolution: float = 0.05 + # optional origin of the 0:0 position + # (mostly imporant for the ROS 2 mapserver) + origin: Vector3 = field(default_factory=Vector3) + + # obstacles/walls, for example useful to model a building + wall: list = field(default_factory=list) # list[Wall] + # a marker is a single points with a direction + marker: list = field(default_factory=list) + # the area is an annotation of a region + area: list = field(default_factory=list) # list[Area] + # the path consists of multiple points connected to each other + path: list = field(default_factory=list) + + # ext is short for external mesh, useful for more complex environments. + ext: list = field(default_factory=list) + + lane_graph: LaneGraph = None + + def __post_init__(self): + if isinstance(self.size, (list, tuple)): + self.size = Dimension(*self.size) + if isinstance(self.origin, list): + # pylint: disable=not-a-mapping + self.origin = Vector3(*self.origin) + if isinstance(self.origin, dict): + # pylint: disable=not-a-mapping + self.origin = Vector3(**self.origin) + if self.marker: + self.marker = [ + Marker(**m) if isinstance(m, dict) else m + for m in self.marker + ] + if self.area: + self.area = [ + Area(**a) if isinstance(a, dict) else a + for a in self.area + ] + if self.wall: + self.wall = [ + Wall(**w) if isinstance(w, dict) else w + for w in self.wall + ] + if self.path: + self.path = [ + Path(**p) if isinstance(p, dict) else p + for p in self.path + ] + + # TODO: self.ext + + if isinstance(self.lane_graph, dict): + self.lane_graph = LaneGraph(**self.lane_graph) + + def __iter__(self): + yield ('name', self.name) + yield ('description', self.description) + if self.wall: + yield ('wall', [dict(w) for w in self.wall]) + yield ('resolution', self.resolution) + yield ('origin', list(self.origin)) + if self.lane_graph: + yield ('lane_graph', dict(self.lane_graph)) + if self.area: + yield ('area', [dict(a) for a in self.area]) + if self.marker: + yield ('marker', [dict(m) for m in self.marker]) + if self.path: + yield ('path', [dict(m) for m in self.path]) + if self.ext: + yield ('ext', [dict(m) for m in self.ext]) + + def _meshes(self): + """returns all items that have a mesh.""" + meshes = [] + if self.area: + meshes += self.area + if self.wall: + meshes += self.wall + return [m for m in meshes if m.type == 'mesh'] + + def recenter(self): + """Recenter walls and areas""" + for item in self._meshes(): + if item.type == 'mesh': + item.data.recenter() + + def boxify(self): + """If there is a mesh with 4 points that align to make it a box""" + for item in self._meshes(): + item.boxify() + + def bounding_box(self): + """recenter walls and areas.""" + for item in self._meshes(): + item.bounding_box() diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/marker.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/marker.py new file mode 100644 index 0000000..cf9eda6 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/marker.py @@ -0,0 +1,41 @@ +# annotations or special points on the map, can for example be used +# by navigation as a goal (similar to lane.LaneNode) +from dataclasses import dataclass, field +from .geom.pose import Pose + + +@dataclass +class Marker: + name: str = 'new marker' + + # marker can have an orientation + pose: Pose = field(default_factory=Pose) + + # additional information about this marker that the user can define + params: dict = field(default_factory=dict) + + # color the marker has on the map, defaults to red + color: list = field(default_factory=lambda: [255, 50, 50]) + + # radius of the marker (if we print the map) + radius: float = 1.0 + + # type of the marker (point, sphere, box, ...) + type: str = 'point' + + def __post_init__(self): + if isinstance(self.pose, dict): + # pylint: disable=not-a-mapping + self.pose = Pose(**self.pose) + + def __iter__(self): + yield ('name', self.name) + yield ('pose', dict(self.pose)) + if self.color: + yield ('color', self.color) + if self.radius is not None: + yield ('radius', self.radius) + if self.params: + yield ('params', self.params) + if self.type: + yield ('type', self.type) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/path.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/path.py new file mode 100644 index 0000000..bc6b5dd --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/path.py @@ -0,0 +1,38 @@ +"""A simple path is just a list of (way-)points. + +For a more defined behavior use lanes (see lane.py). +""" +from dataclasses import dataclass, field +from .geom.pose import Pose +from .geom.vector3 import Vector3 + + +@dataclass +class Path: + name: str + points: list = field(default_factory=list) + pose: Pose = field(default_factory=Pose) + size: Vector3 = None + color: str = 'red' + distance_relative_to_ground: bool = True + radius: float = 0.3 + + def __post_init__(self): + if not self.size: + self.size = Vector3(1, 1, 1) + + def __iter__(self): + yield ('name', self.name) + yield ('points', self.points) + if self.pose: + yield ('pose', dict(self.pose)) + if self.size: + yield ('size', list(self.size)) + if self.color: + yield ('color', self.color) + if self.distance_relative_to_ground: + yield ( + 'distance_relative_to_ground', + self.distance_relative_to_ground) + if self.radius: + yield ('radius', self.radius) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/model/wall.py b/lib/mapdesc_ros/mapdesc/mapdesc/model/wall.py new file mode 100644 index 0000000..0e3d1b1 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/model/wall.py @@ -0,0 +1,78 @@ +from dataclasses import dataclass +from .geom.box import Box +from .geom.mesh import Mesh + + +TYPES = { + 'mesh': Mesh, + 'box': Box +} + + +@dataclass +class Wall: + # required, either a Box or a Mesh + data: Box | Mesh + + # optional name for the wall + name: str = None + + # optional list of holes in the wall + holes: list = None + + # type, is set based on given data + type: str = None + + def __post_init__(self): + if isinstance(self.data, dict): + clz = TYPES[self.type] + # pylint: disable=not-a-mapping + self.data = clz(**self.data) + else: + self.type = self.data.__class__.__name__.lower() + self.holes = self.holes if self.holes else [] + + @property + def center(self): + return self.data.pose.position + + @property + def points(self): + return self.data.points + + @property + def size(self): + return self.data.size + + @property + def pose(self): + return self.data.pose + + def local_points(self): + return self.data.local_points() + + def __iter__(self): + yield ('data', dict(self.data)) + yield ('type', self.type) + if self.name: + yield ('name', self.name) + if self.holes: + yield ('holes', self.holes) + + def bounding_box(self): + if self.type != 'mesh': + return + box = self.data.bounding_box() + if not box: + return + self.data = box + self.type = 'box' + + def boxify(self): + if self.type != 'mesh': + return + box = self.data.boxify() + if not box: + return + self.data = box + self.type = 'box' diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/save/__init__.py b/lib/mapdesc_ros/mapdesc/mapdesc/save/__init__.py new file mode 100644 index 0000000..2631a25 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/save/__init__.py @@ -0,0 +1,14 @@ +from .png import save_png +from .sdf import save_sdf +from .yaml import save_yaml +from .svg import save_svg +from .rosmap import save_rosmap + + +SAVE = { + 'sdf': save_sdf, + 'yaml': save_yaml, + 'png': save_png, + 'svg': save_svg, + 'rosmap': save_rosmap +} diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/save/png.py b/lib/mapdesc_ros/mapdesc/mapdesc/save/png.py new file mode 100644 index 0000000..ea7d4f4 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/save/png.py @@ -0,0 +1,79 @@ +try: + import cv2 + CV2_AVAILABLE = True +except ImportError: + CV2_AVAILABLE = False +import numpy as np +from ..util import image_dimensions +import logging + +logger = logging.getLogger(__name__) + + +def rgb_to_bgr(rgb): + return [rgb[2], rgb[1], rgb[0]] + + +def save_png(_map, output_file, obstacles_only=False): + if not CV2_AVAILABLE: + raise RuntimeError('Can not save as PNG, OpenCV2 not avaialbe') + width, height, offset_x, offset_y = image_dimensions(_map) + res = _map.resolution + img = np.zeros((int(height / res), int(width / res), 3), dtype=np.uint8) + img.fill(255) + if _map.area and not obstacles_only: + for area in _map.area: + pts = [ + ( + int((p.x + offset_x) / res), + int((p.y + offset_y) / res) + ) for p in area.local_points() + ] + pts = np.array( + [pts], + dtype=np.int32) + color = rgb_to_bgr(area.color) if area.color \ + else [255, 100, 100] + cv2.fillPoly(img, [pts], color) + + for wall in _map.wall: + pts = [ + ( + int((p.x + offset_x) / res), + int((p.y + offset_y) / res) + ) for p in wall.local_points() + ] + pts = np.array( + [pts], + dtype=np.int32) + color = [0, 0, 0] + cv2.fillPoly(img, [pts], color) + + if _map.lane_graph and not obstacles_only: + for edge in _map.lane_graph.edges: + s = edge.source.position + t = edge.target.position + x1 = int((s.x + offset_x) / res) + y1 = int((s.y + offset_y) / res) + x2 = int((t.x + offset_x) / res) + y2 = int((t.y + offset_y) / res) + thickness = 1 + color = (0, 0, 255) + start = (x1, y1) + end = (x2, y2) + cv2.line(img, start, end, color, thickness) + + if not obstacles_only: + for marker in _map.marker: + pos = marker.pose.position + x = int((pos.x + offset_x) / res) + y = int((pos.y + offset_y) / res) + color = rgb_to_bgr(marker.color) if marker.color else (0, 0, 255) + radius = (marker.radius if marker.radius else 1.0) / res + cv2.circle(img, (x, y), int(radius), color) + if width == 0 or height == 0: + logger.error( + 'Can not safe image %s, invalid dimensions %i:%i', + output_file, width, height) + return + cv2.imwrite(str(output_file), img) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/save/rosmap.py b/lib/mapdesc_ros/mapdesc/mapdesc/save/rosmap.py new file mode 100644 index 0000000..c497e38 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/save/rosmap.py @@ -0,0 +1,28 @@ +from .png import save_png +from ..model import Map +import yaml +from pathlib import Path +import logging + +logger = logging.getLogger(__name__) + + +def save_rosmap(_map: Map, yaml_file: str): + path = Path(yaml_file) + if path.suffix not in ['.yaml', '.yml']: + logger.error('Not a yaml file!') + return + base = yaml_file[:-len(path.suffix)] + png_file = Path(f'{base}.png') + save_png(_map, png_file, obstacles_only=True) + yaml_data = { + 'image': str(png_file.relative_to(png_file.parent)), + 'resolution': _map.resolution, + 'origin': list(_map.origin), + 'negate': 0, + 'occupied_thresh': 0.65, + 'free_thresh': 0.196 + } + with open(str(yaml_file), 'w', encoding='utf-8') as fd: + yaml.safe_dump( + yaml_data, fd, default_style=None, default_flow_style=None) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/save/sdf.py b/lib/mapdesc_ros/mapdesc/mapdesc/save/sdf.py new file mode 100644 index 0000000..a6445a4 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/save/sdf.py @@ -0,0 +1,24 @@ +import os +import jinja2 +from pathlib import Path + + +BASE_DIR = Path(os.path.dirname(__file__)).absolute() +TEMPLATE_DIR = BASE_DIR.parent / 'data' / 'templates' +templateLoader = jinja2.FileSystemLoader(searchpath=TEMPLATE_DIR) +templateEnv = jinja2.Environment(loader=templateLoader) +TEMPLATE_SDF = "model.sdf.j2" +TEMPLATE_CFG = "model.config.j2" +template_sdf = templateEnv.get_template(TEMPLATE_SDF) +template_cfg = templateEnv.get_template(TEMPLATE_CFG) + + +def save_sdf(_map, output_path): + output_path = Path(output_path) + if not output_path.exists(): + os.mkdir(output_path) + with open(output_path / 'model.sdf', 'w', encoding='UTF-8') as model: + model.write(template_sdf.render(map=_map)) + with open(output_path / 'model.config', 'w', encoding='UTF-8') as model: + author = {'name': 'MapDesc Generator', 'email': 'noone@example.com'} + model.write(template_cfg.render(map=_map, author=author)) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/save/svg.py b/lib/mapdesc_ros/mapdesc/mapdesc/save/svg.py new file mode 100644 index 0000000..53b7e35 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/save/svg.py @@ -0,0 +1,88 @@ +import math +from ..util import image_dimensions + + +SVG_TPL = ''' + + {items} + +''' +WALL_STYLE = 'fill:black;' +SVG_LINE = '' +SVG_CIRCLE = '' +LANE_STYLE = 'stroke:red;stroke-width:1' + + +def save_svg(_map, output_file): + width, height, offset_x, offset_y = image_dimensions(_map) + res = _map.resolution + + svg_data = SVG_TPL.format( + width=math.ceil(width / res), height=math.ceil(height / res), + offset_x=0, offset_y=0, + items='{items}') + + svg_items = [] + if _map.area: + svg_items.append('') + for area in _map.area: + color = area.color if area.color else [100, 100, 255] + style = 'fill:#{:02x}{:02x}{:02x};'.format(*color) + svg_poly = ''.format( + style=style, points=''.join([ + f'{round((p.x + offset_x) / res):.0f},' + f'{round((p.y + offset_y) / res):.0f} ' + for p in area.local_points()])) + svg_items.append(svg_poly) + else: + svg_items.append('') + + if _map.wall: + svg_items.append('') + for wall in _map.wall: + svg_poly = ''.format( + style=WALL_STYLE, points=''.join([ + f'{round((p.x + offset_x) / res):.0f},' + f'{round((p.y + offset_y) / res):.0f} ' + for p in wall.local_points()])) + svg_items.append(svg_poly) + else: + svg_items.append('') + + # TODO: path! + + if _map.lane_graph: + svg_items.append('') + for edge in _map.lane_graph.edges: + s = edge.source.position + t = edge.target.position + x1 = (s.x + offset_x) / res + y1 = (s.y + offset_y) / res + x2 = (t.x + offset_x) / res + y2 = (t.y + offset_y) / res + svg_items.append(SVG_LINE.format( + x1=x1, x2=x2, y1=y1, y2=y2, style=LANE_STYLE)) + else: + svg_items.append('') + + if _map.marker: + svg_items.append('') + for marker in _map.marker: + pos = marker.pose.position + x = int((pos.x + offset_x) / res) + y = int((pos.y + offset_y) / res) + color = marker.color if marker.color else [0, 0, 255] + stroke = '#{:02x}{:02x}{:02x}'.format(*color) + radius = (marker.radius if marker.radius else 1.0) / res + _style = 'fill:none' + svg_items.append(SVG_CIRCLE.format( + style=_style, cx=x, cy=y, r=radius, stroke_width=1, + stroke=stroke)) + else: + svg_items.append('') + + with open(output_file, 'w', encoding='utf-8') as fp: + fp.write(svg_data.format(items='\n '.join(svg_items))) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/save/yaml.py b/lib/mapdesc_ros/mapdesc/mapdesc/save/yaml.py new file mode 100644 index 0000000..f5f69e0 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/save/yaml.py @@ -0,0 +1,8 @@ +import yaml +from ..model import Map + + +def save_yaml(_map: Map, output_file: str): + with open(output_file, 'w', encoding='utf-8') as f: + yaml.safe_dump( + dict(_map), f, default_style=None, default_flow_style=None) diff --git a/lib/mapdesc_ros/mapdesc/mapdesc/util.py b/lib/mapdesc_ros/mapdesc/mapdesc/util.py new file mode 100644 index 0000000..886abe2 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/mapdesc/util.py @@ -0,0 +1,110 @@ +import math + + +def dot_product(p1, p2, p3): + """calculates the dot-product of two vectors from 3 points. + + Useful to determine if it is a right angle.""" + v1 = (p2.x - p1.x, p2.y - p1.y) + v2 = (p3.x - p2.x, p3.y - p2.y) + return v1[0] * v2[0] + v1[1] * v2[1] + + +def calculate_slope(p1, p2): + """calculate slope of line between 2 points.""" + if p1.x == p2.x: + return float('inf') # Use infinity to represent vertical lines + return (p2.y - p1.y) / (p2.x - p1.x) + + +def bounding_box(points: list) -> list: + """get bounding box around all given points.""" + x_min = min([p[0] for p in points]) + y_min = min([p[1] for p in points]) + x_max = max([p[0] for p in points]) + y_max = max([p[1] for p in points]) + return x_min, y_min, x_max, y_max + + +def ccw_sort(points: list): + """sort points counter-clockwise. + + We assume the coordinates are taken from the center and calculate the + degree of each point to order by angle. + """ + points_distance = [ + ( + math.atan2(p.x, p.y) % (math.pi * 2), + p + ) for p in points + ] + points_distance.sort(key=lambda x: x[0], reverse=True) + return [p[1] for p in points_distance] + + +def image_dimensions(_map) -> list: + """Get image dimensions for SVG and PNG export.""" + if _map.size.width > 1 and _map.size.length > 1: + return _map.size.width, _map.size.length, 0, 0 + else: + points = [p for w in _map.wall for p in w.local_points()] + if _map.marker: + # subtract and add radius to get top-left and bottom-right + # corner of the marker + points += [m.pose.position + m.radius for m in _map.marker] + points += [m.pose.position - m.radius for m in _map.marker] + if _map.lane_graph: + points += [n.position for n in _map.lane_graph.nodes] + if not points: + print('WARN: no obstacles given to calculate bounding box') + return 0, 0, 0, 0 + x_min, y_min, x_max, y_max = bounding_box(points) + # image size is calculated by the points, we set the position + # of the image by the top-left and bottom-right bounding box + # that the points form. Only works if we have at least 2 points. + width, height = x_max - x_min, y_max - y_min + # we move all points between min and max, so we need to move all + # points by that (shift all points by an offset so they are inside + # the image) + return width, height, -x_min, -y_min + + +def euler_to_quaternion(roll: float, pitch: float, yaw: float) -> set: + # roll (X), pitch (Y), yaw (Z), + # Abbreviations for the various angular functions based on + # https://en.wikipedia.org/wiki/ + # Conversion_between_quaternions_and_Euler_angles + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + return ( + sr * cp * cy - cr * sp * sy, # x + cr * sp * cy + sr * cp * sy, # y + cr * cp * sy - sr * sp * cy, # z + cr * cp * cy + sr * sp * sy # w + ) + + +def quaternion_to_euler(x: float, y: float, z: float, w: float): + # roll (x-axis rotation) + sinr_cosp = 2 * (w * x + y * z) + cosr_cosp = 1 - 2 * (x * x + y * y) + roll = math.atan2(sinr_cosp, cosr_cosp) + + # pitch (y-axis rotation) + sinp = 2 * (w * y - z * x) + # math.asin has to be between -1 and 1, so we return 90°(π/2) as limit + sinp = max(-1, sinp) + sinp = min(1, sinp) + pitch = math.asin(sinp) + + # yaw (z-axis rotation) + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + yaw = math.atan2(siny_cosp, cosy_cosp) + + return roll, pitch, yaw diff --git a/lib/mapdesc_ros/mapdesc/pytest.bash b/lib/mapdesc_ros/mapdesc/pytest.bash new file mode 100755 index 0000000..7b5da28 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/pytest.bash @@ -0,0 +1,5 @@ +#!/bin/bash +# run pytest and coverage +python3 -m coverage run --source mapdesc -m pytest . +python3 -m coverage report -m +python3 -m coverage html \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/requirements.txt b/lib/mapdesc_ros/mapdesc/requirements.txt new file mode 100644 index 0000000..bc137e2 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/requirements.txt @@ -0,0 +1,6 @@ +argcomplete +pyyaml +jinja2 +imutils +numpy>=1.24 +OSMPythonTools \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/setup.cfg b/lib/mapdesc_ros/mapdesc/setup.cfg new file mode 100644 index 0000000..ac4a813 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/setup.cfg @@ -0,0 +1,21 @@ +[bumpversion] +current_version = 0.1 +commit = True +tag = True + +[bumpversion:file:setup.py] +search = version='{current_version}' +replace = version='{new_version}' + +[bumpversion:file:mapdesc/__init__.py] +search = __version__ = '{current_version}' +replace = __version__ = '{new_version}' + +[bdist_wheel] +universal = 1 + +[flake8] +exclude = docs + +[aliases] +test = pytest \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/setup.py b/lib/mapdesc_ros/mapdesc/setup.py new file mode 100644 index 0000000..6e1d434 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/setup.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +import os +from setuptools import setup, find_packages + + +def package_files(directory): + """add package_files for setup.""" + paths = [] + for (path, _, filenames) in os.walk(directory): + for filename in filenames: + paths.append(os.path.join('..', path, filename)) + return paths + + +EXTRA_FILES = package_files('mapdesc/data') +SHORT_DESC = "Map Description and format conversion for robotics applications." + + +with open("README.md", "r", encoding='utf-8') as fh: + long_description = fh.read() + +setup( + name="mapdesc", + description=SHORT_DESC, + long_description=long_description, + long_description_content_type="text/markdown", + url="https://github.com/dfki-ric/mapdesc", + version="0.1", + license="BSD-3", + author="Andreas Bresser", + packages=find_packages(), + tests_require=[], + include_package_data=True, + package_data={'': EXTRA_FILES}, + install_requires=[ + 'argcomplete', + 'jinja2', + 'pyyaml', + 'imutils', + 'numpy>=1.24', + 'OSMPythonTools' + ], + entry_points={ + 'console_scripts': [ + 'mapdesc = mapdesc.cli:main', + ], + }, +) diff --git a/lib/mapdesc_ros/mapdesc/test/geojson/geojson_drone.json b/lib/mapdesc_ros/mapdesc/test/geojson/geojson_drone.json new file mode 100644 index 0000000..5c3558f --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/geojson/geojson_drone.json @@ -0,0 +1,34 @@ +{ + "type": "FeatureCollection", + "features": [ + { + "type": "Feature", + "properties": {}, + "geometry": { + "coordinates": [ + [ + 8.85854569154364, + 53.11180309646721 + ], + [ + 8.85921278666325, + 53.1115805149382 + ], + [ + 8.859445451991974, + 53.1118074607991 + ], + [ + 8.858734732122826, + 53.11205950023157 + ], + [ + 8.858532967658363, + 53.11181509837945 + ] + ], + "type": "LineString" + } + } + ] + } \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/test/map/mallmap.png b/lib/mapdesc_ros/mapdesc/test/map/mallmap.png new file mode 100755 index 0000000..28b9a37 Binary files /dev/null and b/lib/mapdesc_ros/mapdesc/test/map/mallmap.png differ diff --git a/lib/mapdesc_ros/mapdesc/test/map/mallmap.yaml b/lib/mapdesc_ros/mapdesc/test/map/mallmap.yaml new file mode 100644 index 0000000..56c0d3c --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/map/mallmap.yaml @@ -0,0 +1,6 @@ +image: mallmap.png +resolution: 0.05 +origin: [-20.5, -17.5, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/lib/mapdesc_ros/mapdesc/test/simple_walls_sdf/model.config b/lib/mapdesc_ros/mapdesc/test/simple_walls_sdf/model.config new file mode 100644 index 0000000..8750280 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/simple_walls_sdf/model.config @@ -0,0 +1,15 @@ + + + Simple Walls + 1.0 + model.sdf + + + MapDesc Generator + noone@example.com + + + + A simple room with 4 walls and an entrance for a door + + \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/test/simple_walls_sdf/model.sdf b/lib/mapdesc_ros/mapdesc/test/simple_walls_sdf/model.sdf new file mode 100644 index 0000000..6f0b626 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/simple_walls_sdf/model.sdf @@ -0,0 +1,128 @@ + + + + 0 0 0 0 0 0 + + + + + + 0.2 20 3 + + + 0 0 1.5 0 0 0 + + + + + 0.2 20 3 + + + 0 0 1.5 0 0 0 + + + 1 1 1 1 + + + 0 + + + -10 0 1.5 0 0 0 + + + + + + + 0.2 20 3 + + + 0 0 1.5 0 0 0 + + + + + 0.2 20 3 + + + 0 0 1.5 0 0 0 + + + 1 1 1 1 + + + 0 + + + 10 0 1.5 0.0 0.0 0.0 + + + + + + + 0.2 20 3 + + + 0 0 1.5 0 0 0 + + + + + 0.2 20 3 + + + 0 0 1.5 0 0 0 + + + 1 1 1 1 + + + 0 + + + 0 10 1.5 0.0 0.0 1.570494235572534 + + + + + + + 0.2 20 3 + + + 0 0 1.5 0 0 0 + + + + + 0.2 20 3 + + + 0 0 1.5 0 0 0 + + + 1 1 1 1 + + + 0 + + + 0 -10 1.5 0.0 0.0 1.570494235572534 + + + 1 + + \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/test/test_conversions.py b/lib/mapdesc_ros/mapdesc/test/test_conversions.py new file mode 100644 index 0000000..6f1a929 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_conversions.py @@ -0,0 +1,187 @@ +from mapdesc.model.geom import Vector2, Vector3, Quaternion, Dimension + + +def test_from_to_list(): + vec = Vector2(*[1, 2, 'ignore_me']) + assert vec.x == 1 + assert vec.y == 2 + assert list(vec) == [1.0, 2.0] + + vec = Vector3(*[1, 2, 3, 'ignore_me']) + assert vec.x == 1 + assert vec.y == 2 + assert vec.z == 3 + assert list(vec) == [1.0, 2.0, 3.0] + + quat = Quaternion(*[1, 2, 3, 4, 'ignore_me']) + assert quat.x == 1 + assert quat.y == 2 + assert quat.z == 3 + assert quat.w == 4 + assert list(quat) == [1.0, 2.0, 3.0, 4.0] + + dim = Dimension(*[1, 2, 3, 'ignore_me']) + assert dim.width == 1 + assert dim.length == 2 + assert dim.height == 3 + assert list(dim) == [1.0, 2.0, 3.0] + + +def test_null(): + num = [1, 2, 3, 4, 'ignore_me'] + for clz in [Vector2, Vector3, Quaternion]: + vec = clz(*num) + assert not vec.null() + vec = clz() + assert vec.null() + + dim = Dimension(*[1, 2, 3]) + assert not dim.null() + dim = Dimension() + assert not dim.null() + dim = Dimension(0, 0, 0) + assert dim.null() + + +def test_sub(): + num = [10, 9, 8, 7, 'ignore_me'] + + vec = Vector2(*num) - 3 + assert vec.x == 7 + assert vec.y == 6 + vec -= Vector2(1, 1) + assert vec.x == 6 + assert vec.y == 5 + + vec = Vector3(*num) - 2 + assert vec.x == 8 + assert vec.y == 7 + assert vec.z == 6 + vec -= Vector3(1, 1, 1) + assert vec.x == 7 + assert vec.y == 6 + assert vec.z == 5 + + quat = Quaternion(*num) - 2 + assert quat.x == 8 + assert quat.y == 7 + assert quat.z == 6 + assert quat.w == 5 + quat -= Quaternion(-1, 1, -2, -3) + assert quat.x == 9 + assert quat.y == 6 + assert quat.z == 8 + assert quat.w == 8 + + dim = Dimension(*num) - 2 + assert dim.width == 8 + assert dim.length == 7 + assert dim.height == 6 + dim -= Dimension(-2, -4, -6) + assert dim.width == 10 + assert dim.length == 11 + assert dim.height == 12 + dim -= dim + assert dim.null() + + +def test_mul(): + num = [1, 2, 3, 4, 'ignore_me'] + + vec = Vector2(*num) * 2 + assert vec.x == 2 + assert vec.y == 4 + vec *= vec + assert vec.x == 4 + assert vec.y == 16 + + vec = Vector3(*num) * 2 + assert vec.x == 2 + assert vec.y == 4 + assert vec.z == 6 + vec *= vec + assert vec.x == 4 + assert vec.y == 16 + assert vec.z == 36 + + quat = Quaternion(*num) * 2 + assert quat.x == 2 + assert quat.y == 4 + assert quat.z == 6 + assert quat.w == 8 + quat *= quat + assert quat.x == 4 + assert quat.y == 16 + assert quat.z == 36 + assert quat.w == 64 + + dim = Dimension(*num) * 2 + assert dim.width == 2 + assert dim.length == 4 + assert dim.height == 6 + dim *= dim + assert dim.width == 4 + assert dim.length == 16 + assert dim.height == 36 + + +def test_add(): + num = [1, 2, 3, 4, 'ignore_me'] + + vec = Vector2(*num) + 2 + assert vec.x == 3 + assert vec.y == 4 + vec += vec + assert vec.x == 6 + assert vec.y == 8 + + vec = Vector3(*num) + 4 + assert vec.x == 5 + assert vec.y == 6 + assert vec.z == 7 + vec += vec + assert vec.x == 10 + assert vec.y == 12 + assert vec.z == 14 + + quat = Quaternion(*num) + 6 + assert quat.x == 7 + assert quat.y == 8 + assert quat.z == 9 + assert quat.w == 10 + quat += quat + assert quat.x == 14 + assert quat.y == 16 + assert quat.z == 18 + assert quat.w == 20 + + dim = Dimension(*num) + 2 + assert dim.width == 3 + assert dim.length == 4 + assert dim.height == 5 + dim += dim + assert dim.width == 6 + assert dim.length == 8 + assert dim.height == 10 + + +def test_neg(): + vec = -Vector2(*[1, 2, 'ignore_me']) + assert vec.x == -1 + assert vec.y == -2 + + vec = -Vector3(*[1, 2, 3, 'ignore_me']) + assert vec.x == -1 + assert vec.y == -2 + assert vec.z == -3 + + quat = -Quaternion(*[1, 2, 3, 4, 'ignore_me']) + assert quat.x == -1 + assert quat.y == -2 + assert quat.z == -3 + assert quat.w == -4 + + dim = -Dimension(*[1, 2, 3, 'ignore_me']) + assert dim.width == -1 + assert dim.length == -2 + assert dim.height == -3 diff --git a/lib/mapdesc_ros/mapdesc/test/test_dimension.py b/lib/mapdesc_ros/mapdesc/test/test_dimension.py new file mode 100644 index 0000000..8c982f1 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_dimension.py @@ -0,0 +1,8 @@ +from mapdesc.model.geom import Dimension + + +def test_dimension(): + dim = Dimension() + assert dim.length == 1 + assert dim.height == 1 + assert dim.width == 1 diff --git a/lib/mapdesc_ros/mapdesc/test/test_lanes.py b/lib/mapdesc_ros/mapdesc/test/test_lanes.py new file mode 100644 index 0000000..f1af573 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_lanes.py @@ -0,0 +1,23 @@ +from mapdesc.model import LaneGraph + + +def test_lane_from_dict(): + graph_data = { + 'nodes': [ + {'position': [1, 1, 1]}, + {'position': [2, 2, 2]}, + {'position': [3, 3, 3]} + ], + 'edges': [ + {'edge_type': 0, 'source': 0, 'target': 1}, + {'edge_type': 0, 'source': 1, 'target': 2} + ] + } + graph = LaneGraph(**graph_data) + assert graph.nodes[0].position.x == 1 + assert graph.edges[1].target.name == 'node_2' + idx = 0 + for node in graph_data['nodes']: + node['name'] = f'node_{idx}' + idx += 1 + assert dict(graph) == graph_data diff --git a/lib/mapdesc_ros/mapdesc/test/test_load_save.py b/lib/mapdesc_ros/mapdesc/test/test_load_save.py new file mode 100644 index 0000000..70bec88 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_load_save.py @@ -0,0 +1,38 @@ +from mapdesc.load.rosmap import load_rosmap +from mapdesc.load.yaml import load_yaml +from mapdesc.save import save_sdf, save_png, save_svg, save_rosmap +from pathlib import Path + + +def test_gen_sdf(): + _map = load_rosmap('./test/map/mallmap.yaml') + assert _map.wall + save_sdf(_map, './generated/sdf/mallmap_unittest_sdf1') + + +def test_gen_sdf2(): + _map = load_yaml('./test/yaml/simple_walls.yaml') + save_sdf(_map, './generated/sdf/mallmap_unittest_sdf2') + + +def test_gen_png(): + mall_map = load_rosmap('./test/map/mallmap.yaml') + assert mall_map.wall + save_png(mall_map, './generated/mallmap_unittest_png.png') + + hdp_map = load_yaml('./test/yaml/hdp_2_agents_map.yml') + assert hdp_map.wall + save_png(hdp_map, './generated/hdp_unittest_png.png') + + +def test_gen_svg(): + _map = load_yaml('./test/yaml/hdp_2_agents_map.yml') + save_svg(_map, './generated/hdp2_unittest_svg.svg') + + +def test_rosmap(): + _map = load_yaml('./test/yaml/hdp_2_agents_map.yml') + save_rosmap(_map, './generated/fail.test') + save_rosmap(_map, './generated/hdp_rosmap.yaml') + assert Path('./generated/hdp_rosmap.png').exists() + assert Path('./generated/hdp_rosmap.yaml').exists() diff --git a/lib/mapdesc_ros/mapdesc/test/test_map.py b/lib/mapdesc_ros/mapdesc/test/test_map.py new file mode 100644 index 0000000..5f59292 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_map.py @@ -0,0 +1,7 @@ +from mapdesc.load.yaml import load_yaml + + +def test_map_to_dict(): + _map = load_yaml('./test/yaml/hdp_2_agents_map.yml') + dict_map = dict(_map) + assert len(dict_map['wall']) == 15 diff --git a/lib/mapdesc_ros/mapdesc/test/test_mesh.py b/lib/mapdesc_ros/mapdesc/test/test_mesh.py new file mode 100644 index 0000000..d8b3c30 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_mesh.py @@ -0,0 +1,19 @@ +from mapdesc.model.geom.mesh import Mesh + + +def test_recenter(): + mesh = Mesh( + polygons=[ + (1, 2), + (11, 2), + (11, 11), + (1, 11) + ] + ) + mesh.recenter() + assert mesh.pose.position.x == 6 + assert mesh.pose.position.y == 6.5 + assert mesh.pose.position.z == 0 + + assert mesh.polygons[0].x == -5 + assert mesh.polygons[0].y == -4.5 diff --git a/lib/mapdesc_ros/mapdesc/test/test_plane.py b/lib/mapdesc_ros/mapdesc/test/test_plane.py new file mode 100644 index 0000000..de9aaed --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_plane.py @@ -0,0 +1,11 @@ +from mapdesc.model.geom import Dimension, Plane + + +def test_plane(): + plane = Plane( + normal=[1, 1, 1], + size=[2, 2]) + assert isinstance(plane.size, Dimension) + assert plane.size.height == 0 + plane_dict = dict(plane) + assert plane_dict['size'] == (2, 2) diff --git a/lib/mapdesc_ros/mapdesc/test/test_pose.py b/lib/mapdesc_ros/mapdesc/test/test_pose.py new file mode 100644 index 0000000..da80d49 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_pose.py @@ -0,0 +1,60 @@ +import math +import pytest +from mapdesc.model.geom import Pose, Vector2, Vector3, Quaternion +from mapdesc.model.geom.pose import any_to_vector, dict_to_vector + + +def test_euler_orientation(): + p = Pose(orientation=Vector3( + math.pi / 180 * 90, + math.pi / 180 * 45, + math.pi / 180 * 180)) + euler = p.euler_orientation() + assert tuple(euler) == (math.pi / 2, math.pi / 4, math.pi) + # check caching + euler = p.euler_orientation() + assert tuple(euler) == (math.pi / 2, math.pi / 4, math.pi) + + p = Pose(orientation=Quaternion(0, 0, 0, 1)) + euler = p.euler_orientation() + assert tuple(euler) == (0, 0, 0) + + p = Pose(orientation=Quaternion(1, 0, 0, 0)) + euler = p.euler_orientation() + assert tuple(euler) == (math.pi, 0, 0) + + p = Pose(orientation=Quaternion(0.707, 0, 0, 0.707)) + euler = p.euler_orientation() + assert tuple(euler) == (pytest.approx(math.pi / 2, .001), 0, 0) + + +def test_pose_dict(): + p = Pose() + # test post-init + assert isinstance(p.orientation, Quaternion) + # test __iter__ + assert '_euler_orientation' not in dict(p) + assert 'orientation' in dict(p) + # note that dict("custom_object") calls __iter__() on all nested + # structures (like position and orientation). + assert dict(p)['orientation'] == (0, 0, 0, 1) + + +def test_any_to_vector(): + my_dict = {'x': 1, 'y': 2} + vec2 = any_to_vector(my_dict) + assert isinstance(vec2, Vector2) + + my_dict['z'] = 3 + vec3 = any_to_vector(my_dict) + assert isinstance(vec3, Vector3) + + my_dict['w'] = 4 + quat = any_to_vector(my_dict) + assert isinstance(quat, Quaternion) + + my_dict = {'x': 1} + with pytest.raises(RuntimeError) as exc_info: + dict_to_vector(my_dict) + err_str = 'x and y not set, not a valid pose.' + assert exc_info.value.args[0] == err_str diff --git a/lib/mapdesc_ros/mapdesc/test/test_quaternion.py b/lib/mapdesc_ros/mapdesc/test/test_quaternion.py new file mode 100644 index 0000000..173548d --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_quaternion.py @@ -0,0 +1,99 @@ +from mapdesc.model.geom import Box, Dimension, Pose, \ + Quaternion, Vector2, Mesh +import pytest + + +def test_from_any(): + original = Quaternion(1, 2, 3, 1) + vec = Quaternion.from_any([1, 2, 3, 1]) + assert vec.to_tuple() == original.to_tuple() + vec = Quaternion.from_any({'x': 1, 'y': 2, 'z': 3, 'w': 1}) + assert vec.to_tuple() == original.to_tuple() + try: + Quaternion.from_any('invalid') + assert False + except RuntimeError: + pass + + +def test_rotaion(): + # test to rotate a mesh + # 1. rotate simple box by 45 degree [] + box = Box( + size=Dimension( + width=2.0, length=5.0, height=1.0 + ), + pose=Pose( + # roate by 45 degree + orientation=Quaternion(z=0.383, w=0.924) + ) + ) + # apply roation + points = box.local_points() + points = [round(p, 3) for p in points] + assert Vector2(1.062, -2.474) in points + assert Vector2(2.475, -1.059) in points + assert Vector2(-1.062, 2.474) in points + assert Vector2(-2.475, 1.059) in points + + # 2. rotate complex shape by 23 degree [] + polygons = [ + Vector2(-20, -30), + Vector2(10, -40), + Vector2(50, -20), + Vector2(100, 60), + Vector2(-70, 10), + Vector2(-10, 0) + ] + + mesh = Mesh( + polygons=polygons, + pose=Pose( + position=Vector2(80, 100), + # roate by 23 + orientation=Quaternion(z=0.2, w=0.98) + )) + points = mesh.local_points() + points = [round(p, 3) for p in points] + assert points == [ + Vector2(x=73.355, y=64.562), + Vector2(x=104.874, y=67.117), + Vector2(x=133.838, y=101.192), + Vector2(x=148.493, y=194.386), + Vector2(x=11.679, y=81.771), + Vector2(x=70.8, y=96.082)] + + +def test_getitem(): + q = Quaternion(x=3, y=4, z=5, w=6) + assert q['w'] == 6 + assert q[2] == 5 + assert q[0] == q['x'] == 3 + assert q[1] == 4 + with pytest.raises(RuntimeError) as exc_info: + assert not q['a'] # a should not be a valid subscription + assert exc_info.value.args[0] == 'unknown key "a"' + with pytest.raises(RuntimeError) as exc_info: + assert q[-1] + assert exc_info.value.args[0] == 'unknown key "-1"' + + +def test_copy(): + q = Quaternion(x=3, y=4, z=5, w=6) + q2 = q.copy() + q2.x = 1 + q2.y = 2 + q2.z = 3 + q2.w = 4 + assert q.x == 3 + assert q.y == 4 + assert q.z == 5 + assert q.w == 6 + + +def test_normalize(): + vec = Quaternion(0, 0, 0, 0) + with pytest.raises(RuntimeError) as exc_info: + vec.normalize() + assert exc_info.value.args[0].startswith( + 'Can not normalize null quaternion') diff --git a/lib/mapdesc_ros/mapdesc/test/test_serialization.py b/lib/mapdesc_ros/mapdesc/test/test_serialization.py new file mode 100644 index 0000000..46d537e --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_serialization.py @@ -0,0 +1,10 @@ +import json +from mapdesc.model import Marker + + +def test_to_json(): + model = Marker() + assert json.dumps(dict(model)) == '{"name": "new marker", '\ + '"pose": {"orientation": [0.0, 0.0, 0.0, 1.0], "position": '\ + '[0.0, 0.0, 0.0]}, "color": [255, 50, 50], "radius": 1.0, '\ + '"type": "point"}' diff --git a/lib/mapdesc_ros/mapdesc/test/test_utils.py b/lib/mapdesc_ros/mapdesc/test/test_utils.py new file mode 100644 index 0000000..575f4f5 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_utils.py @@ -0,0 +1,35 @@ +import math +from mapdesc import util +from mapdesc.model.geom import Quaternion, Vector2, Mesh +import pytest + + +def test_quaternion(): + assert \ + (0.0, 0.0, 0.707107, 0.707107) == \ + pytest.approx(util.euler_to_quaternion(0, 0, math.pi / 180 * 90)) + + quat = Quaternion(0, 0, 0.707107, 0.707107) + euler = quat.to_euler() + assert euler.x == 0 + assert euler.y == 0 + assert euler.z == pytest.approx(math.pi / 180 * 90) + + +def test_sort_points(): + # these points are clockwise sort + cw_points = [Vector2(x, y) for x, y in [[0, 0], [0, 1], [1, 0], [1, 1]]] + # these are counter clockwise + # TODO: check, if this is correct, we probably don't want to go + # from 0.0 to 0.1! + ccw_points = [Vector2(x, y) for x, y in [[1, 0], [1, 1], [0, 0], [0, 1]]] + assert util.ccw_sort(cw_points) == ccw_points + + mesh = Mesh(polygons=cw_points) + mesh.ccw_sort() + assert mesh.polygons == ccw_points + + +def test_bounding_box(): + points = [[1, 2], [-3, 99], [-6, -100]] + assert util.bounding_box(points) == (-6, -100, 1, 99) diff --git a/lib/mapdesc_ros/mapdesc/test/test_vec2.py b/lib/mapdesc_ros/mapdesc/test/test_vec2.py new file mode 100644 index 0000000..b96e433 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_vec2.py @@ -0,0 +1,60 @@ +from mapdesc.model.geom import Vector2 +import json + + +def test_is_close(): + a = Vector2(1, 2) + b = Vector2(3, 4) + assert not a.is_close(b) + assert not a.is_close(None) + assert not a.is_close({'x': 3, 'y': 5}) + assert not a.is_close({3, 5}) + assert a.is_close({3, 5}, threshold=10) + + +def test_distance(): + a = Vector2(1, 2) + b = Vector2(3, 4) + assert a.distance(b) == 2.8284271247461903 + + +def test_normalize(): + a = Vector2(1, 2).normalize() + # we are using the euclidean length, so we do not normalize based on + # min and max (so the solution is not 0.5, 1.0) + assert a.x == 0.4472135954999579 + assert a.y == 0.8944271909999159 + + +def test_serialize(): + a = Vector2(1, 2) + assert "[1.0, 2.0]" == json.dumps(a.serialize()) + + +def test_from_any(): + original = Vector2(1, 2) + vec = Vector2.from_any([1, 2]) + assert vec.to_tuple() == original.to_tuple() + vec = Vector2.from_any({'x': 1, 'y': 2}) + assert vec.to_tuple() == original.to_tuple() + try: + Vector2.from_any('invalid') + assert False + except RuntimeError: + pass + + +def test_copy(): + vec = Vector2(x=3, y=4) + vec2 = vec.copy() + vec2.x = 1 + vec2.y = 2 + assert vec.x == 3 + assert vec.y == 4 + + +def test_round(): + vec = Vector2(1.1111111, 2.22222222) + vec2 = round(vec, 2) + assert vec.x == 1.1111111 + assert vec2 == Vector2(1.11, 2.22) diff --git a/lib/mapdesc_ros/mapdesc/test/test_vec3.py b/lib/mapdesc_ros/mapdesc/test/test_vec3.py new file mode 100644 index 0000000..972359d --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_vec3.py @@ -0,0 +1,56 @@ +from mapdesc.model.geom import Vector3 + + +def test_is_close(): + a = Vector3(1, 2, 5) + b = Vector3(3, 4, 6) + assert not a.is_close(b) + assert not a.is_close(None) + assert not a.is_close({'x': 3, 'y': 5, 'z': 8}) + assert not a.is_close({3, 5, 6}) + assert a.is_close({3, 5, 6}, threshold=10) + assert not a.is_close({3, 11, 10}, threshold=10) + + +def test_to_tuple(): + a = Vector3(1, 2, 5) + # check inherited to_tuple + assert a.to_tuple() == tuple((1.0, 2.0, 5.0)) + + +def test_normalize(): + a = Vector3(1, 2, 3).normalize() + assert a.x == 0.2672612419124244 + assert a.y == 0.5345224838248488 + assert a.z == 0.8017837257372732 + + +def test_from_any(): + original = Vector3(1, 2, 3) + vec = Vector3.from_any([1, 2, 3]) + assert vec.to_tuple() == original.to_tuple() + vec = Vector3.from_any({'x': 1, 'y': 2, 'z': 3}) + assert vec.to_tuple() == original.to_tuple() + try: + Vector3.from_any('invalid') + assert False + except RuntimeError: + pass + + +def test_copy(): + vec = Vector3(x=3, y=4, z=5) + vec2 = vec.copy() + vec2.x = 1 + vec2.y = 2 + vec2.z = 3 + assert vec.x == 3 + assert vec.y == 4 + assert vec.z == 5 + + +def test_round(): + vec = Vector3(1.1111111, 2.22222222, 3.333333) + vec2 = round(vec, 2) + assert vec.x == 1.1111111 + assert vec2 == Vector3(1.11, 2.22, 3.33) diff --git a/lib/mapdesc_ros/mapdesc/test/test_wall.py b/lib/mapdesc_ros/mapdesc/test/test_wall.py new file mode 100644 index 0000000..d9f443a --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_wall.py @@ -0,0 +1,15 @@ +from mapdesc import model + + +def test_wall_box(): + wall = model.Wall(data=model.geom.Box()) + # assure a default length is set + assert wall.data.size.length == 1.0 + # assure the type is the given box + assert wall.type == 'box' + + +def test_wall_mesh(): + wall = model.Wall(data=model.geom.Mesh()) + assert wall.type == 'mesh' + assert wall.data.size.width == 1.0 diff --git a/lib/mapdesc_ros/mapdesc/test/test_yaml_import.py b/lib/mapdesc_ros/mapdesc/test/test_yaml_import.py new file mode 100644 index 0000000..58cef88 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/test_yaml_import.py @@ -0,0 +1,20 @@ +import os +from pathlib import Path +from mapdesc import load + + +BASE_PATH = Path(os.path.dirname(__file__)).absolute() + + +def test_load_yaml(): + _map = load.load_yaml( + input_file=BASE_PATH / 'yaml' / 'simple_walls.yaml') + assert len(_map.wall) == 4 + assert _map.wall[0].pose.position.x == -10 + + # Hi Digit Pro 4.0 production map as complex example + _map = load.load_yaml( + input_file=BASE_PATH / 'yaml' / 'hdp_2_agents_map.yml') + assert len(_map.area) == 7 + assert len(_map.lane_graph.edges) == 74 + assert len(_map.lane_graph.nodes) == 23 diff --git a/lib/mapdesc_ros/mapdesc/test/yaml/demonstrator_map.yml b/lib/mapdesc_ros/mapdesc/test/yaml/demonstrator_map.yml new file mode 100644 index 0000000..a5f0a53 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/yaml/demonstrator_map.yml @@ -0,0 +1,274 @@ +area: + # Demonstrator Area + - name: demonstation_area + color: [255, 200, 200] + type: mesh + data: + polygons: + - [-34, 5] + - [21, 5] + - [21, 23] + - [-34, 23] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + + # Storage Areas + - name: components_storage + color: [100, 100, 255] + type: mesh + data: + polygons: + - [6, -25] + - [40, -25] + - [40, -8] + - [6, -8] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + + - name: parts_storage + color: [255, 165, 0] + type: mesh + data: + polygons: + - [-16, -25] + - [5, -25] + - [5, -8] + - [-16, -8] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + + # Arrival Areas + - name: components_arrival_area + color: [173, 216, 230] + type: mesh + data: + polygons: + - [21, 5] + - [38, 5] + - [38, 23] + - [21, 23] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + + - name: delivery_drop_off_area + color: [173, 216, 230] + type: mesh + data: + polygons: + - [-36, -23] + - [-21, -23] + - [-21, -10] + - [-36, -10] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + + + # Tools and Parts Boxes + - name: tools_box + color: [0, 0, 139] + type: mesh + data: + polygons: + - [22, 21] + - [24, 21] + - [24, 23] + - [22, 23] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + + - name: parts_box + color: [0, 100, 0] + type: mesh + data: + polygons: + - [25, 21] + - [28, 21] + - [28, 23] + - [25, 23] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + +wall: + # Main Walls + # Main Wall 00 + - data: + polygons: + - [40, -25] + - [41, -25] + - [41, 25] + - [40, 25] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Main Wall 01 + - data: + polygons: + - [-41, 25] + - [41, 25] + - [41, 26] + - [-41, 26] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Main Wall 02 + - data: + polygons: + - [-41, 23] + - [-40, 23] + - [-40, 25] + - [-41, 25] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Main Wall 03 + - data: + polygons: + - [-41, -13] + - [-40, -13] + - [-40, 5] + - [-41, 5] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Main Wall 04 + - data: + polygons: + - [-41, -25] + - [-40, -25] + - [-40, -23] + - [-41, -23] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Main Wall 05 + - data: + polygons: + - [-41, -26] + - [41, -26] + - [41, -25] + - [-41, -25] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Components Storage + # Wall 00 + - data: + polygons: + - [38, -8] + - [40, -8] + - [40, -7] + - [38, -7] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Wall 01 + - data: + polygons: + - [5, -8] + - [21, -8] + - [21, -7] + - [5, -7] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Wall 02 + - data: + polygons: + - [5, -25] + - [6, -25] + - [6, -7] + - [5, -7] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Parts Storage + # Wall 00 + - data: + polygons: + - [3, -8] + - [5, -8] + - [5, -7] + - [3, -7] + + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Wall 01 + - data: + polygons: + - [-16, -8] + - [-5, -8] + - [-5, -7] + - [-16, -7] + + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Wall 02 + - data: + polygons: + - [-17, -25] + - [-16, -25] + - [-16, -7] + - [-17, -7] + + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Demonstation Walls + # Wall bottom + - data: + polygons: + - [-34, 22] + - [21, 22] + - [21, 23] + - [-34, 23] + + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh + + # Wall top + - data: + polygons: + - [-34, 5] + - [21, 5] + - [21, 6] + - [-34, 6] + + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/test/yaml/hdp_2_agents_map.yml b/lib/mapdesc_ros/mapdesc/test/yaml/hdp_2_agents_map.yml new file mode 100644 index 0000000..44fa807 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/yaml/hdp_2_agents_map.yml @@ -0,0 +1,469 @@ +area: +- area_type: storage + color: [100, 255, 100] + data: + polygons: + - [-34.375, 0.0] + - [-15.625, 0.0] + - [-15.625, 12.5] + - [-34.375, 12.5] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] +- area_type: storage + color: [100, 255, 100] + data: + polygons: + - [-34.375, -12.5] + - [-15.625, -12.5] + - [-15.625, 0.0] + - [-34.375, 0.0] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] +- area_type: storage + color: [100, 255, 100] + data: + polygons: + - [15.625, -12.5] + - [34.375, -12.5] + - [34.375, 12.5] + - [15.625, 12.5] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] +- area_type: assembly + color: [100, 100, 255] + data: + polygons: + - [2.5, 2.5] + - [10.0, 2.5] + - [10.0, 10.0] + - [2.5, 10.0] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] +- area_type: assembly + color: [100, 100, 255] + data: + polygons: + - [-10.0, 2.5] + - [-2.5, 2.5] + - [-2.5, 10.0] + - [-10.0, 10.0] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] +- area_type: assembly + color: [100, 100, 255] + data: + polygons: + - [-10.0, -10.0] + - [-2.5, -10.0] + - [-2.5, -2.5] + - [-10.0, -2.5] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] +- area_type: assembly + color: [100, 100, 255] + data: + polygons: + - [2.5, -10.0] + - [10.0, -10.0] + - [10.0, -2.5] + - [2.5, -2.5] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] +description: Some map. +lane_graph: + edges: + - {edge_type: 1, source: 0, target: 20} + - {edge_type: 1, source: 0, target: 4} + - {edge_type: 1, source: 0, target: 15} + - {edge_type: 1, source: 0, target: 3} + - {edge_type: 1, source: 0, target: 2} + - {edge_type: 1, source: 0, target: 1} + - {edge_type: 1, source: 1, target: 8} + - {edge_type: 1, source: 1, target: 18} + - {edge_type: 1, source: 1, target: 0} + - {edge_type: 1, source: 1, target: 5} + - {edge_type: 1, source: 1, target: 13} + - {edge_type: 1, source: 2, target: 0} + - {edge_type: 1, source: 2, target: 5} + - {edge_type: 1, source: 2, target: 6} + - {edge_type: 1, source: 2, target: 20} + - {edge_type: 1, source: 3, target: 0} + - {edge_type: 1, source: 3, target: 7} + - {edge_type: 1, source: 3, target: 22} + - {edge_type: 1, source: 3, target: 6} + - {edge_type: 1, source: 3, target: 17} + - {edge_type: 1, source: 4, target: 0} + - {edge_type: 1, source: 4, target: 8} + - {edge_type: 1, source: 4, target: 15} + - {edge_type: 1, source: 4, target: 7} + - {edge_type: 1, source: 5, target: 18} + - {edge_type: 1, source: 5, target: 2} + - {edge_type: 1, source: 5, target: 1} + - {edge_type: 1, source: 5, target: 9} + - {edge_type: 1, source: 6, target: 22} + - {edge_type: 1, source: 6, target: 3} + - {edge_type: 1, source: 6, target: 2} + - {edge_type: 1, source: 6, target: 10} + - {edge_type: 1, source: 7, target: 3} + - {edge_type: 1, source: 7, target: 4} + - {edge_type: 1, source: 7, target: 12} + - {edge_type: 1, source: 7, target: 17} + - {edge_type: 1, source: 8, target: 4} + - {edge_type: 1, source: 8, target: 11} + - {edge_type: 1, source: 8, target: 13} + - {edge_type: 1, source: 8, target: 1} + - {edge_type: 1, source: 9, target: 5} + - {edge_type: 1, source: 10, target: 6} + - {edge_type: 1, source: 11, target: 8} + - {edge_type: 1, source: 11, target: 12} + - {edge_type: 1, source: 12, target: 11} + - {edge_type: 1, source: 12, target: 7} + - {edge_type: 1, source: 13, target: 8} + - {edge_type: 1, source: 13, target: 14} + - {edge_type: 1, source: 13, target: 1} + - {edge_type: 1, source: 14, target: 15} + - {edge_type: 1, source: 14, target: 13} + - {edge_type: 1, source: 15, target: 0} + - {edge_type: 1, source: 15, target: 4} + - {edge_type: 1, source: 15, target: 14} + - {edge_type: 1, source: 15, target: 16} + - {edge_type: 1, source: 16, target: 15} + - {edge_type: 1, source: 16, target: 17} + - {edge_type: 1, source: 17, target: 3} + - {edge_type: 1, source: 17, target: 16} + - {edge_type: 1, source: 17, target: 7} + - {edge_type: 1, source: 18, target: 5} + - {edge_type: 1, source: 18, target: 19} + - {edge_type: 1, source: 18, target: 1} + - {edge_type: 1, source: 19, target: 20} + - {edge_type: 1, source: 19, target: 18} + - {edge_type: 1, source: 20, target: 0} + - {edge_type: 1, source: 20, target: 19} + - {edge_type: 1, source: 20, target: 2} + - {edge_type: 1, source: 20, target: 21} + - {edge_type: 1, source: 21, target: 22} + - {edge_type: 1, source: 21, target: 20} + - {edge_type: 1, source: 22, target: 3} + - {edge_type: 1, source: 22, target: 6} + - {edge_type: 1, source: 22, target: 21} + nodes: + - name: node_1 + position: {x: 0.0, y: 0.0} + - name: node_2 + position: {x: 0.0, y: 11.25} + - name: node_3 + position: {x: -12.5, y: 0.0} + - name: node_4 + position: {x: 0.0, y: -11.25} + - name: node_5 + position: {x: 12.5, y: 0.0} + - name: node_6 + position: {x: -12.5, y: 11.25} + - name: node_7 + position: {x: -12.5, y: -11.25} + - name: node_8 + position: {x: 12.5, y: -11.25} + - name: node_9 + position: {x: 12.5, y: 11.25} + - name: node_10 + position: {x: -25.0, y: 6.25} + - name: node_11 + position: {x: -25.0, y: -6.25} + - name: node_12 + position: {x: 25.0, y: 6.25} + - name: node_13 + position: {x: 25.0, y: -6.25} + - name: node_14 + position: {x: 6.25, y: 11.25} + - name: node_15 + position: {x: 6.25, y: 6.25} + - name: node_16 + position: {x: 6.25, y: 0.0} + - name: node_17 + position: {x: 6.25, y: -6.25} + - name: node_18 + position: {x: 6.25, y: -11.25} + - name: node_19 + position: {x: -6.25, y: 11.25} + - name: node_20 + position: {x: -6.25, y: 6.25} + - name: node_21 + position: {x: -6.25, y: 0.0} + - name: node_22 + position: {x: -6.25, y: -6.25} + - name: node_23 + position: {x: -6.25, y: -11.25} +marker: +- color: [0, 0, 255] + name: actor_1 + params: {actor_type: wing_spawn, type: actor} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [-29.6875, 3.125] +- color: [0, 0, 255] + name: actor_2 + params: {actor_type: wing_spawn, type: actor} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [-29.6875, 6.25] +- color: [0, 0, 255] + name: actor_3 + params: {actor_type: wing_spawn, type: actor} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [-29.6875, 9.375] +- color: [0, 0, 255] + name: actor_4 + params: {actor_type: movable_spawn, type: actor} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [-29.6875, -9.375] +- color: [0, 0, 255] + name: actor_5 + params: {actor_type: movable_spawn, type: actor} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [-29.6875, -6.25] +- color: [0, 0, 255] + name: actor_6 + params: {actor_type: movable_spawn, type: actor} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [-29.6875, -3.125] +- color: [0, 0, 255] + name: actor_7 + params: {actor_type: despawn, type: actor} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [29.6875, 0.0] +- color: [255, 50, 50] + name: agent_1 + params: {agent_type: mir_100, type: agent_spawn} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0] +- color: [255, 50, 50] + name: agent_2 + params: {agent_type: mir_100, type: agent_spawn} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [-1.25, 0.0] +- color: [255, 50, 50] + name: actor_3 + params: {agent_type: generic_arm, type: agent_spawn} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [8.125, 6.25] +- color: [255, 50, 50] + name: actor_4 + params: {agent_type: generic_arm, type: agent_spawn} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [-8.125, 6.25] +- color: [255, 50, 50] + name: actor_5 + params: {agent_type: generic_arm, type: agent_spawn} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [-8.125, -6.25] +- color: [255, 50, 50] + name: actor_6 + params: {agent_type: generic_arm, type: agent_spawn} + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [8.125, -6.25] +name: some_graph +origin: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] +resolution: 0.05 +wall: +- data: + polygons: + - [2.375, 2.375] + - [5.125, 2.375] + - [5.125, 2.6250000000000004] + - [2.6250000000000004, 2.6250000000000004] + - [2.6250000000000004, 9.875] + - [5.125, 9.875] + - [5.125, 10.125] + - [2.375, 10.125] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [7.375, 2.375] + - [10.125, 2.375] + - [10.125, 10.125] + - [7.375, 10.125] + - [7.375, 9.875] + - [9.874999999999998, 9.875] + - [9.874999999999998, 2.6250000000000004] + - [7.375, 2.6250000000000004] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [-10.125, 2.375] + - [-7.375, 2.375] + - [-7.375, 2.6250000000000004] + - [-9.874999999999998, 2.6250000000000004] + - [-9.874999999999998, 9.875] + - [-7.375, 9.875] + - [-7.375, 10.125] + - [-10.125, 10.125] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [-5.125, 2.375] + - [-2.375, 2.375] + - [-2.375, 10.125] + - [-5.125, 10.125] + - [-5.125, 9.875] + - [-2.6250000000000004, 9.875] + - [-2.6250000000000004, 2.6250000000000004] + - [-5.125, 2.6250000000000004] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [-10.125, -10.125] + - [-7.375, -10.125] + - [-7.375, -9.875] + - [-9.874999999999998, -9.875] + - [-9.874999999999998, -2.6250000000000004] + - [-7.375, -2.6250000000000004] + - [-7.375, -2.375] + - [-10.125, -2.375] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [-5.125, -10.125] + - [-2.375, -10.125] + - [-2.375, -2.375] + - [-5.125, -2.375] + - [-5.125, -2.6250000000000004] + - [-2.6250000000000004, -2.6250000000000004] + - [-2.6250000000000004, -9.875] + - [-5.125, -9.875] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [2.375, -10.125] + - [5.125, -10.125] + - [5.125, -9.875] + - [2.6250000000000004, -9.875] + - [2.6250000000000004, -2.6250000000000004] + - [5.125, -2.6250000000000004] + - [5.125, -2.375] + - [2.375, -2.375] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [7.375, -10.125] + - [10.125, -10.125] + - [10.125, -2.375] + - [7.375, -2.375] + - [7.375, -2.6250000000000004] + - [9.874999999999998, -2.6250000000000004] + - [9.874999999999998, -9.875] + - [7.375, -9.875] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [-34.375, 12.375] + - [34.375, 12.375] + - [34.375, 12.625] + - [-34.375, 12.625] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [-34.375, -12.625] + - [34.375, -12.625] + - [34.375, -12.375] + - [-34.375, -12.375] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [34.25, -12.5] + - [34.5, -12.5] + - [34.5, 12.5] + - [34.25, 12.5] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [-34.5, -12.5] + - [-34.25, -12.5] + - [-34.25, 12.5] + - [-34.5, 12.5] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [-34.375, -0.125] + - [-15.625, -0.125] + - [-15.625, 0.125] + - [-34.375, 0.125] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [-15.75, -7.5] + - [-15.5, -7.5] + - [-15.5, 7.5] + - [-15.75, 7.5] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh +- data: + polygons: + - [15.5, -7.5] + - [15.75, -7.5] + - [15.75, 7.5] + - [15.5, 7.5] + pose: + orientation: [0.0, 0.0, 0.0, 1.0] + position: [0.0, 0.0, 0.0] + type: mesh diff --git a/lib/mapdesc_ros/mapdesc/test/yaml/simple_walls.yaml b/lib/mapdesc_ros/mapdesc/test/yaml/simple_walls.yaml new file mode 100644 index 0000000..ee776dd --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test/yaml/simple_walls.yaml @@ -0,0 +1,27 @@ +name: "Simple Walls" +description: "A simple room with 4 walls and an entrance for a door" +wall: + - type: "box" + data: + pose: + position: [-10, 0, 1.5] + orientation: [0, 0, 0] # x y z - euler + size: [0.2, 20, 3] + - type: "box" + data: + pose: + position: [10, 0, 1.5] + orientation: [0, 0, 0, 1] # x y z w - quaternion + size: [0.2, 20, 3] + - type: "box" + data: + pose: + position: [0, 10, 1.5] + orientation: [0, 0, 0.707, 0.707] + size: [0.2, 20, 3] + - type: "box" + data: + pose: + position: [0, -10, 1.5] + orientation: [0, 0, 0.707, 0.707] + size: [0.2, 20, 3] \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc/test_cli.bash b/lib/mapdesc_ros/mapdesc/test_cli.bash new file mode 100755 index 0000000..7f84603 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc/test_cli.bash @@ -0,0 +1,11 @@ +#!/bin/bash +mapdesc yaml test/yaml/simple_walls.yaml sdf ./generated/sdf/test1 +mapdesc rosmap test/map/mallmap.yaml yaml ./generated/mallmap.yml +mapdesc rosmap test/map/mallmap.yaml yaml -b ./generated/mallmap_bounding_box.yml +mapdesc rosmap test/map/mallmap.yaml sdf ./generated/sdf/test2 +mapdesc rosmap test/map/mallmap.yaml png ./generated/test1.png +mapdesc yaml test/yaml/hdp_2_agents_map.yml rosmap ./generated/hdp_rosmap.yaml +mapdesc yaml --recenter test/yaml/demonstrator_map.yml yaml ./generated/demonstrator_map_recenter.yml +mapdesc yaml --boxify test/yaml/demonstrator_map.yml yaml ./generated/demonstrator_map_box.yml +# mapdesc osm 53.0762098 8.8075270 80 earth svg ./generated/bremen_city.svg +# mapdesc osm 53.0762098 8.8075270 80 earth yaml ./generated/bremen_city.yml \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/CMakeLists.txt b/lib/mapdesc_ros/mapdesc_msgs/CMakeLists.txt new file mode 100644 index 0000000..4380c50 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/CMakeLists.txt @@ -0,0 +1,60 @@ +cmake_minimum_required(VERSION 3.8) +project(mapdesc_msgs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +find_package(std_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rclcpp REQUIRED) + +find_package(rosidl_default_generators REQUIRED) +ament_export_dependencies(rosidl_default_runtime) + +rosidl_generate_interfaces(${PROJECT_NAME} "msg/Area.msg" + "msg/Box.msg" + "msg/Dimension.msg" + "msg/External.msg" + "msg/LaneEdge.msg" + "msg/LaneGraph.msg" + "msg/LaneNode.msg" + "msg/Map.msg" + "msg/Marker.msg" + "msg/Mesh.msg" + "msg/Path.msg" + "msg/Wall.msg" + "srv/MapAreaCreate.srv" + "srv/MapAreaDelete.srv" + "srv/MapAreaList.srv" + "srv/MapAreaUpdate.srv" + "srv/MapCreate.srv" + "srv/MapDelete.srv" + "srv/MapExtCreate.srv" + "srv/MapExtDelete.srv" + "srv/MapExtList.srv" + "srv/MapExtUpdate.srv" + "srv/MapGet.srv" + "srv/MapList.srv" + "srv/MapMarkerCreate.srv" + "srv/MapMarkerDelete.srv" + "srv/MapMarkerList.srv" + "srv/MapMarkerUpdate.srv" + "srv/MapOverwrite.srv" + "srv/MapPathCreate.srv" + "srv/MapPathDelete.srv" + "srv/MapPathList.srv" + "srv/MapPathUpdate.srv" + "srv/MapUpdate.srv" + "srv/MapWallCreate.srv" + "srv/MapWallDelete.srv" + "srv/MapWallList.srv" + "srv/MapWallUpdate.srv" + DEPENDENCIES diagnostic_msgs std_msgs geometry_msgs) + +ament_package() diff --git a/lib/mapdesc_ros/mapdesc_msgs/README.md b/lib/mapdesc_ros/mapdesc_msgs/README.md new file mode 100644 index 0000000..8fb3801 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/README.md @@ -0,0 +1,7 @@ +# Map Description ROS 2 Messages +ROS 2 messages and services for the Map Description ROS 2 Wrapper. + + +## Motivation +Part of [Map Description ROS 2 Wrapper](../mapdesc_ros) but as own repository so you don't have to check out nor install the whole Map-Desc +with all its requirements (like OpenCV2) if you only want to talk to the Map Description ROS 2 wrapper from another machine. diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/Area.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/Area.msg new file mode 100644 index 0000000..1ab05f0 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/Area.msg @@ -0,0 +1,5 @@ +string name +string type +string area_type +uint8[] color +Mesh data \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/Box.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/Box.msg new file mode 100644 index 0000000..3ab9d9e --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/Box.msg @@ -0,0 +1,4 @@ +# should be box +string type +Dimension size +geometry_msgs/Pose pose \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/Dimension.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/Dimension.msg new file mode 100644 index 0000000..462685c --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/Dimension.msg @@ -0,0 +1,3 @@ +float64 width +float64 height +float64 length \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/External.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/External.msg new file mode 100644 index 0000000..2a17523 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/External.msg @@ -0,0 +1,4 @@ +string name +string type +Mesh data +string[] filenames \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/LaneEdge.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/LaneEdge.msg new file mode 100644 index 0000000..30b6d4a --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/LaneEdge.msg @@ -0,0 +1,5 @@ +LaneNode source +LaneNode target +int8 edge_type +string name +diagnostic_msgs/KeyValue params \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/LaneGraph.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/LaneGraph.msg new file mode 100644 index 0000000..8b807d5 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/LaneGraph.msg @@ -0,0 +1,2 @@ +LaneNode[] nodes +LaneEdge[] edges \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/LaneNode.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/LaneNode.msg new file mode 100644 index 0000000..bba2d37 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/LaneNode.msg @@ -0,0 +1,3 @@ +string name +geometry_msgs/Vector3 position +diagnostic_msgs/KeyValue params \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/Map.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/Map.msg new file mode 100644 index 0000000..ab8c989 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/Map.msg @@ -0,0 +1,13 @@ +string name +string description + +Dimension size +float64 resolution +geometry_msgs/Vector3 origin + +Marker[] marker +Area[] area +Wall[] wall +Path[] path +External[] ext +LaneGraph lane_graph \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/Marker.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/Marker.msg new file mode 100644 index 0000000..aa18f7d --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/Marker.msg @@ -0,0 +1,5 @@ +geometry_msgs/Pose pose +string name +uint8[] color +string type +float32 radius \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/Mesh.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/Mesh.msg new file mode 100644 index 0000000..68753e3 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/Mesh.msg @@ -0,0 +1,5 @@ +# mesh +geometry_msgs/Vector3[] polygons +# box +Dimension size +geometry_msgs/Pose pose \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/Path.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/Path.msg new file mode 100644 index 0000000..cdd13e6 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/Path.msg @@ -0,0 +1,7 @@ +string name +int64[] points +geometry_msgs/Pose pose +geometry_msgs/Vector3 size +string color +bool distance_relative_to_ground +float64 radius \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/msg/Wall.msg b/lib/mapdesc_ros/mapdesc_msgs/msg/Wall.msg new file mode 100644 index 0000000..580d267 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/msg/Wall.msg @@ -0,0 +1,3 @@ +Mesh data +string name +string type \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/package.xml b/lib/mapdesc_ros/mapdesc_msgs/package.xml new file mode 100644 index 0000000..929d251 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/package.xml @@ -0,0 +1,29 @@ + + + + mapdesc_msgs + 0.0.1 + Messages for MapDesc. + Andreas Bresser + BSD-3 + + rclcpp + diagnostic_msgs + geometry_msgs + std_msgs + + builtin_interfaces + rosidl_default_runtime + + rosidl_default_generators + ament_cmake + + rosidl_default_generators + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaCreate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaCreate.srv new file mode 100644 index 0000000..03ef2e8 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaCreate.srv @@ -0,0 +1,7 @@ +# This file has been generated by ROSCRUD + +# Create area on map with unique identifier (name) +string name +Area item +--- +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaDelete.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaDelete.srv new file mode 100644 index 0000000..8b2cf56 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaDelete.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Delete area at index on map +string name +uint32 index +--- +# Is False if map.name did not exist but was requested to delete or index out of range +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaList.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaList.srv new file mode 100644 index 0000000..d3997a9 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaList.srv @@ -0,0 +1,6 @@ +# This file has been generated by ROSCRUD + +# get all area from map by name. +string name +--- +Area[] area \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaUpdate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaUpdate.srv new file mode 100644 index 0000000..6633ea4 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapAreaUpdate.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Update on map at given index +string name +Area item +uint32 index +--- +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapCreate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapCreate.srv new file mode 100644 index 0000000..d6cc4db --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapCreate.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Create map with unique identifier (name) +Map map +--- +# is False if map.name already existed. +# use MapUpdate.srv instead for updating an existing map +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapDelete.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapDelete.srv new file mode 100644 index 0000000..25fff90 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapDelete.srv @@ -0,0 +1,7 @@ +# This file has been generated by ROSCRUD + +# Delete map +string name +--- +# Is False if map.name did not exist but was requested to delete +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtCreate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtCreate.srv new file mode 100644 index 0000000..d0f2f2f --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtCreate.srv @@ -0,0 +1,7 @@ +# This file has been generated by ROSCRUD + +# Create ext on map with unique identifier (name) +string name +External item +--- +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtDelete.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtDelete.srv new file mode 100644 index 0000000..a2b91d9 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtDelete.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Delete ext at index on map +string name +uint32 index +--- +# Is False if map.name did not exist but was requested to delete or index out of range +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtList.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtList.srv new file mode 100644 index 0000000..9aea42f --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtList.srv @@ -0,0 +1,6 @@ +# This file has been generated by ROSCRUD + +# get all ext from map by name. +string name +--- +External[] ext \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtUpdate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtUpdate.srv new file mode 100644 index 0000000..b8bacbf --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapExtUpdate.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Update on map at given index +string name +External item +uint32 index +--- +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapGet.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapGet.srv new file mode 100644 index 0000000..afbcc05 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapGet.srv @@ -0,0 +1,6 @@ +# This file has been generated by ROSCRUD + +# Get single map by its unique identifier (name) +string name +--- +Map map \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapList.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapList.srv new file mode 100644 index 0000000..0285cb8 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapList.srv @@ -0,0 +1,5 @@ +# This file has been generated by ROSCRUD + +--- +# get a list of all map +Map[] map \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerCreate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerCreate.srv new file mode 100644 index 0000000..2c8f9a6 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerCreate.srv @@ -0,0 +1,7 @@ +# This file has been generated by ROSCRUD + +# Create marker on map with unique identifier (name) +string name +Marker item +--- +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerDelete.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerDelete.srv new file mode 100644 index 0000000..4691a71 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerDelete.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Delete marker at index on map +string name +uint32 index +--- +# Is False if map.name did not exist but was requested to delete or index out of range +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerList.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerList.srv new file mode 100644 index 0000000..6f9b8ea --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerList.srv @@ -0,0 +1,6 @@ +# This file has been generated by ROSCRUD + +# get all marker from map by name. +string name +--- +Marker[] marker \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerUpdate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerUpdate.srv new file mode 100644 index 0000000..422ee30 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapMarkerUpdate.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Update on map at given index +string name +Marker item +uint32 index +--- +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapOverwrite.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapOverwrite.srv new file mode 100644 index 0000000..ce8e06d --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapOverwrite.srv @@ -0,0 +1,4 @@ +# This file has been generated by ROSCRUD + +bool allow_overwrite +--- \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathCreate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathCreate.srv new file mode 100644 index 0000000..53f889f --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathCreate.srv @@ -0,0 +1,7 @@ +# This file has been generated by ROSCRUD + +# Create path on map with unique identifier (name) +string name +Path item +--- +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathDelete.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathDelete.srv new file mode 100644 index 0000000..014f2dd --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathDelete.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Delete path at index on map +string name +uint32 index +--- +# Is False if map.name did not exist but was requested to delete or index out of range +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathList.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathList.srv new file mode 100644 index 0000000..545fd2e --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathList.srv @@ -0,0 +1,6 @@ +# This file has been generated by ROSCRUD + +# get all path from map by name. +string name +--- +Path[] path \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathUpdate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathUpdate.srv new file mode 100644 index 0000000..1b4258e --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapPathUpdate.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Update on map at given index +string name +Path item +uint32 index +--- +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapUpdate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapUpdate.srv new file mode 100644 index 0000000..4679d0f --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapUpdate.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Update map +Map map +--- +# is False if map.name does not exist. +# use MapCreate.srv instead to create a new map entry. +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallCreate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallCreate.srv new file mode 100644 index 0000000..e469040 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallCreate.srv @@ -0,0 +1,7 @@ +# This file has been generated by ROSCRUD + +# Create wall on map with unique identifier (name) +string name +Wall item +--- +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallDelete.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallDelete.srv new file mode 100644 index 0000000..8b3a272 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallDelete.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Delete wall at index on map +string name +uint32 index +--- +# Is False if map.name did not exist but was requested to delete or index out of range +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallList.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallList.srv new file mode 100644 index 0000000..82e9bbc --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallList.srv @@ -0,0 +1,6 @@ +# This file has been generated by ROSCRUD + +# get all wall from map by name. +string name +--- +Wall[] wall \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallUpdate.srv b/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallUpdate.srv new file mode 100644 index 0000000..2fe3892 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_msgs/srv/MapWallUpdate.srv @@ -0,0 +1,8 @@ +# This file has been generated by ROSCRUD + +# Update on map at given index +string name +Wall item +uint32 index +--- +bool success \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_ros/integration_tests/data/simple_marker_map.yml b/lib/mapdesc_ros/mapdesc_ros/integration_tests/data/simple_marker_map.yml new file mode 100644 index 0000000..9692f6c --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/integration_tests/data/simple_marker_map.yml @@ -0,0 +1,124 @@ +{ + name: simple_map_marker, + description: A simple room with 4 walls and a big entrance., + marker: + [ + { + name: center_marker, + pose: { position: [ 0, 0, 0 ], orientation: [ 0, 0, 0, 1 ] }, + color: [ 255, 50, 50 ], + radius: 0.1 + }, + { + name: north_west, + radius: 0.2, + color: [ 255, 0, 0 ], + pose: + { + position: [ -6.788245960015084, 7.485528022376871, 0 ], + orientation: [ 0, 0, 0, 1 ] + } + }, + { + name: north_east, + radius: 0.2, + color: [ 255, 0, 0 ], + pose: + { + position: [ 7.239428524401198, 8.223826737355047, 0 ], + orientation: [ 0, 0, 0, 1 ] + } + }, + { + name: south_east, + radius: 0.2, + color: [ 255, 0, 0 ], + pose: + { + position: [ 6.829261498693715, -7.936709309296662, 0 ], + orientation: [ 0, 0, 0, 1 ] + } + } + ], + area: + [ + { + area_type: demonstation_area, + color: [ 255, 200, 200 ], + type: mesh, + data: + { + polygons: [ [ -7, -5 ], [ -2, -5 ], [ -2, 5 ], [ -7, 5 ] ], + pose: { orientation: [ 0, 0, 0, 1 ], position: [ -1, -1, 0 ] }, + size: [ 1, 1, 1 ] + }, + name: "" + }, + { + area_type: demonstation_area, + color: [ 100, 200, 200 ], + type: mesh, + data: + { + polygons: [ [ 7, -5 ], [ 4.5, -7 ], [ 2, -5 ], [ 2, 5 ], [ 4.5, 7 ], [ 7, 5 ] ], + pose: { orientation: [ 0, 0, 0, 1 ], position: [ 1, 1, 0 ] }, + size: [ 1, 1, 1 ] + }, + name: "" + }, + { + area_type: outside, + name: outside, + type: box, + data: + { + pose: { position: [ 0, 17, 0 ], orientation: [ 0, 0, 0, 1 ] }, + size: [ 12, 10, 2 ] + }, + color: [ 100, 200, 170 ] + } + ], + wall: + [ + { + type: box, + data: + { + pose: { position: [ 10, 0, 1.5 ], orientation: [ 0, 0, 0, 1 ] }, + size: [ 0.2, 20, 3 ] + } + }, + { + type: box, + data: + { + pose: { position: [ -10, 0, 1.5 ], orientation: [ 0, 0, 0, 1 ] }, + size: [ 0.2, 20, 3 ] + } + }, + { + type: box, + data: + { + pose: { position: [ 0, 10, 1.5 ], orientation: [ 0, 0, 0.7071068, 0.7071068 ] }, + size: [ 0.2, 20, 3 ] + } + }, + { + type: box, + data: + { + pose: { position: [ 7.5, -10, 1.5 ], orientation: [ 0, 0, 0.7071068, 0.7071068 ] }, + size: [ 0.2, 5, 3 ] + } + }, + { + type: box, + data: + { + pose: { position: [ -7.5, -10, 1.5 ], orientation: [ 0, 0, 0.7071068, 0.7071068 ] }, + size: [ 0.2, 5, 3 ] + } + } + ] +} diff --git a/lib/mapdesc_ros/mapdesc_ros/integration_tests/launch_testing/marker_launch_test.py b/lib/mapdesc_ros/mapdesc_ros/integration_tests/launch_testing/marker_launch_test.py new file mode 100755 index 0000000..7ed2510 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/integration_tests/launch_testing/marker_launch_test.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python +# based on https://github.com/ros2/launch_ros/blob/master/ +# launch_testing_ros/test/examples/check_msgs_launch_test.py + +# ROS 2 basics +from rclpy.task import Future +import rclpy + +# ROS 2 launchfile +import launch +import launch.actions +import launch_ros.actions +import launch_testing.actions + +# unit testing and pytest +import pytest +from threading import Event +from threading import Thread +import unittest + +# launch_testing +from launch_testing.io_handler import ActiveIoHandler +import launch_testing.markers + +# custom messages and services +from pathlib import Path +from mapdesc_msgs.srv import MapMarkerCreate, MapMarkerList, MapOverwrite + +BASE_PATH = Path(__file__).parent.absolute() + + +@pytest.mark.launch_test +@launch_testing.markers.keep_alive +def generate_test_description(): + map_yaml = str(BASE_PATH.parent / 'data' / 'simple_marker_map.yml') + return launch.LaunchDescription([ + launch_ros.actions.Node( + package='mapdesc_ros', + executable='mapdesc_service', + name='mapdesc_node', + parameters=[ + {'map_yaml': map_yaml} + ] + ), + launch_testing.actions.ReadyToTest() + ]) + + +class TestFixture(unittest.TestCase): + def marker_added_callback(self, future: Future): + """Callback that gets executed when the marker has been added.""" + assert future.result().success + self.add_marker_service_success_event.set() + + def marker_listed_callback(self, future: Future): + known_marker = [ + 'center_marker', 'north_west', 'north_east', 'south_east'] + marker = future.result().marker + marker_names = [m.name for m in marker] + if len(marker) == len(known_marker): + assert marker_names == known_marker + elif len(marker) == len(known_marker)+1: + # the other test has been executed first + assert marker_names == known_marker + ['new point'] + else: + assert False, 'number of stored marker does not match' + self.list_marker_service_success_event.set() + + def spin(self): + try: + while rclpy.ok() and not self.spinning.is_set(): + rclpy.spin_once(self.node, timeout_sec=0.1) + finally: + return + + def setUp(self): + rclpy.init() + self.node = rclpy.create_node('test_node') + self.list_marker_service_success_event = Event() + self.add_marker_service_success_event = Event() + self.spinning = Event() + # Add a spin thread + self.ros_spin_thread = Thread(target=self.spin) + self.ros_spin_thread.start() + + def wait_for_service(self, service_clz, service_name): + """wait for a service to become available.""" + self.cli = self.node.create_client(service_clz, service_name) + service_available = False + for _try in range(10): + if self.cli.wait_for_service(timeout_sec=.5): + service_available = True + self.node.get_logger().info( + f'service {service_name} is available 👍!') + break + self.node.get_logger().info( + f'service "{service_name}" not available, waiting again...') + + if not service_available: + raise RuntimeError(f'Service "{service_name}" not available ☠!') + + def list_marker(self): + """check if loaded marker are in the list.""" + self.wait_for_service(MapMarkerList, 'mapdesc/marker/list') + request = MapMarkerList.Request(name='simple_map_marker') + future = self.cli.call_async(request) + future.add_done_callback(self.marker_listed_callback) + + def check_overwrite_map(self): + self.wait_for_service(MapOverwrite, '/mapdesc/map/overwrite') + + def add_marker(self): + """test if we can add a new marker""" + self.wait_for_service(MapMarkerCreate, 'mapdesc/marker/create') + request = MapMarkerCreate.Request(name='simple_map_marker') + marker = request.item + marker.name = 'new point' + marker.pose.position.x = 10.0 + marker.pose.position.y = 12.0 + marker.pose.position.z = 0.5 + future = self.cli.call_async(request) + future.add_done_callback(self.marker_added_callback) + + def tearDown(self): + self.spinning.set() + self.ros_spin_thread.join() + self.node.destroy_client(self.cli) + self.node.destroy_node() + rclpy.shutdown() + + def test_check_if_service_called(self, proc_output: ActiveIoHandler): + self.add_marker() + service_called = self.add_marker_service_success_event.wait( + timeout=15.0) + assert service_called, 'Service to add marker not called!' + + def test_check_marker_listed(self, proc_output: ActiveIoHandler): + self.list_marker() + marker_listed = self.list_marker_service_success_event.wait( + timeout=15.0) + assert marker_listed, 'Service to list marker not called!' diff --git a/lib/mapdesc_ros/mapdesc_ros/launch/mapdesc.launch.py b/lib/mapdesc_ros/mapdesc_ros/launch/mapdesc.launch.py new file mode 100644 index 0000000..090ce28 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/launch/mapdesc.launch.py @@ -0,0 +1,33 @@ +from pathlib import Path +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def launch_setup(context, *args, **kwargs): + """ + Generate launch description to start the wrapper for the mapdesc library. + """ + map_yaml = LaunchConfiguration('map_yaml').perform(context) + return [ + LaunchDescription([ + Node( + package='mapdesc_ros', + executable='mapdesc_service', + name='mapdesc_node', + parameters=[ + {'map_yaml': map_yaml} + ] + ) + ])] + + +def generate_launch_description(): + map_yaml = str(Path('/map_data') / 'rh1_eg.yml') + return LaunchDescription([ + DeclareLaunchArgument( + "map_yaml", default_value=map_yaml), + OpaqueFunction(function=launch_setup) + ]) diff --git a/lib/mapdesc_ros/mapdesc_ros/mapdesc_ros/__init__.py b/lib/mapdesc_ros/mapdesc_ros/mapdesc_ros/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/lib/mapdesc_ros/mapdesc_ros/mapdesc_ros/convert_data.py b/lib/mapdesc_ros/mapdesc_ros/mapdesc_ros/convert_data.py new file mode 100644 index 0000000..48b5772 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/mapdesc_ros/convert_data.py @@ -0,0 +1,136 @@ +from geometry_msgs.msg import Pose as PoseMsg +from geometry_msgs.msg import Point as PointMsg +from geometry_msgs.msg import Vector3 as Vector3Msg +from geometry_msgs.msg import Quaternion as QuaternionMsg + +from mapdesc_msgs.msg import Area as AreaMsg +from mapdesc_msgs.msg import Box as BoxMsg +from mapdesc_msgs.msg import Map as MapMsg +from mapdesc_msgs.msg import Marker as MarkerMsg +from mapdesc_msgs.msg import Mesh as MeshMsg + +# mapdesc +from mapdesc.model.geom.pose import Pose +from mapdesc.model.geom.vector3 import Vector3 +from mapdesc.model.geom.quaternion import Quaternion + +from mapdesc.model.geom.box import Box +from mapdesc.model.geom.mesh import Mesh + +from mapdesc.model.area import Area +from mapdesc.model.map import Map +from mapdesc.model.marker import Marker + + +def pose_ros_to_mapdesc(pose: PoseMsg) -> Pose: + return Pose( + position=Vector3( + x=pose.position.x, + y=pose.position.y, + z=pose.position.z), + orientation=Quaternion( + x=pose.orientation.x, + y=pose.orientation.y, + z=pose.orientation.z, + w=pose.orientation.w) + ) + + +def pose_mapdesc_to_ros(pose: Pose) -> PoseMsg: + return PoseMsg( + position=PointMsg( + x=float(pose.position.x), + y=float(pose.position.y), + z=float(pose.position.z)), + orientation=QuaternionMsg( + x=float(pose.orientation.x), + y=float(pose.orientation.y), + z=float(pose.orientation.z), + w=float(pose.orientation.w)) + ) + + +def mesh_ros_to_mapdesc(data: MeshMsg | BoxMsg) -> Mesh | Box: + """Convert mapdesc_msg/MeshBox to mapdesc.model.Mesh/mapdesc.model.Box + """ + pose = pose_ros_to_mapdesc(data.pose) + if isinstance(data, MeshMsg): + return Mesh( + pose=pose, + polygons=data.polygons + ) + elif isinstance(data, BoxMsg): + return Box( + pose=pose, + size=data.size + ) + else: + raise RuntimeError('Not a valid ros mapdesc_msg mesh/box') + + +def mesh_mapdesc_to_ros(data: Mesh | Box) -> MeshMsg | BoxMsg: + """Convert mapdesc_msg/MeshBox to mapdesc.model.Mesh/mapdesc.model.Box + """ + if isinstance(data, Mesh): + return MeshMsg( + pose=pose_mapdesc_to_ros(data.pose), + polygons=[Vector3Msg( + x=float(p.x), y=float(p.y), z=0.0) + for p in data.polygons] + ) + elif isinstance(data, Box): + return BoxMsg( + pose=pose_mapdesc_to_ros(data.pose) + ) + else: + raise RuntimeError('Not a valid mapdesc mesh/box geometry') + + +def area_ros_to_mapdesc(area: AreaMsg) -> Area: + assert isinstance(area, AreaMsg) + return Area( + name=area.name, + type=area.type, + area_type=area.area_type, + color=area.color, + data=mesh_ros_to_mapdesc(area.data) + ) + + +def area_mapdesc_to_ros(area: Area) -> AreaMsg: + assert isinstance(area, Area) + return AreaMsg( + name=area.name, + type=area.type, + area_type=area.area_type, + color=area.color, + data=mesh_mapdesc_to_ros(area.data) + ) + + +def marker_ros_to_mapdesc(marker: MarkerMsg) -> Marker: + return Marker( + name=marker.name, + pose=pose_ros_to_mapdesc(marker.pose), + color=marker.color, + type=marker.type, + radius=marker.radius + ) + + +def marker_mapdesc_to_ros(marker: Marker) -> MarkerMsg: + return MarkerMsg( + name=marker.name, + pose=pose_mapdesc_to_ros(marker.pose), + color=marker.color, + type=marker.type, + radius=float(marker.radius) + ) + + +def map_ros_to_mapdesc(_map: Map): + return MapMsg( + name=_map.name, + marker=[marker_mapdesc_to_ros(m) for m in _map.marker], + # area=[area_mapdesc_to_ros(a) for a in _map.area] + ) diff --git a/lib/mapdesc_ros/mapdesc_ros/mapdesc_ros/map_desc.py b/lib/mapdesc_ros/mapdesc_ros/mapdesc_ros/map_desc.py new file mode 100755 index 0000000..9a8a27b --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/mapdesc_ros/map_desc.py @@ -0,0 +1,355 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String +from mapdesc_msgs.srv import \ + MapCreate, \ + MapDelete, \ + MapGet, \ + MapList, \ + MapUpdate, \ + MapOverwrite, \ + MapMarkerCreate, \ + MapMarkerDelete, \ + MapMarkerList, \ + MapMarkerUpdate, \ + MapAreaCreate, \ + MapAreaDelete, \ + MapAreaList, \ + MapAreaUpdate, \ + MapWallCreate, \ + MapWallDelete, \ + MapWallList, \ + MapWallUpdate, \ + MapPathCreate, \ + MapPathDelete, \ + MapPathList, \ + MapPathUpdate, \ + MapExtCreate, \ + MapExtDelete, \ + MapExtList, \ + MapExtUpdate +from mapdesc_msgs.msg import \ + Area, \ + External, \ + Map, \ + Marker, \ + Path, \ + Wall + +# This file has initially been generated using ROSCRUD + +PREFIX = 'mapdesc' + +# ROS CRUD services +MAP_CREATE = f'{PREFIX}/create' +MAP_DELETE = f'{PREFIX}/delete' +MAP_GET = f'{PREFIX}/get' +MAP_LIST = f'{PREFIX}/list' +MAP_UPDATE = f'{PREFIX}/update' +MAP_OVERWRITE = f'{PREFIX}/overwrite' +MAP_MARKER_CREATE = f'{PREFIX}/marker/create' +MAP_MARKER_DELETE = f'{PREFIX}/marker/delete' +MAP_MARKER_LIST = f'{PREFIX}/marker/list' +MAP_MARKER_UPDATE = f'{PREFIX}/marker/update' +MAP_AREA_CREATE = f'{PREFIX}/area/create' +MAP_AREA_DELETE = f'{PREFIX}/area/delete' +MAP_AREA_LIST = f'{PREFIX}/area/list' +MAP_AREA_UPDATE = f'{PREFIX}/area/update' +MAP_WALL_CREATE = f'{PREFIX}/wall/create' +MAP_WALL_DELETE = f'{PREFIX}/wall/delete' +MAP_WALL_LIST = f'{PREFIX}/wall/list' +MAP_WALL_UPDATE = f'{PREFIX}/wall/update' +MAP_PATH_CREATE = f'{PREFIX}/path/create' +MAP_PATH_DELETE = f'{PREFIX}/path/delete' +MAP_PATH_LIST = f'{PREFIX}/path/list' +MAP_PATH_UPDATE = f'{PREFIX}/path/update' +MAP_EXT_CREATE = f'{PREFIX}/ext/create' +MAP_EXT_DELETE = f'{PREFIX}/ext/delete' +MAP_EXT_LIST = f'{PREFIX}/ext/list' +MAP_EXT_UPDATE = f'{PREFIX}/ext/update' + + +# ROS Topics to inform data change +MAP_ADDED = f'{PREFIX}/added' +MAP_CHANGED = f'{PREFIX}/changed' +MAP_REMOVED = f'{PREFIX}/removed' +MAP_MARKER_ADDED = f'{PREFIX}/marker/added' +MAP_MARKER_CHANGED = f'{PREFIX}/marker/changed' +MAP_MARKER_REMOVED = f'{PREFIX}/marker/removed' +MAP_AREA_ADDED = f'{PREFIX}/area/added' +MAP_AREA_CHANGED = f'{PREFIX}/area/changed' +MAP_AREA_REMOVED = f'{PREFIX}/area/removed' +MAP_WALL_ADDED = f'{PREFIX}/wall/added' +MAP_WALL_CHANGED = f'{PREFIX}/wall/changed' +MAP_WALL_REMOVED = f'{PREFIX}/wall/removed' +MAP_PATH_ADDED = f'{PREFIX}/path/added' +MAP_PATH_CHANGED = f'{PREFIX}/path/changed' +MAP_PATH_REMOVED = f'{PREFIX}/path/removed' +MAP_EXT_ADDED = f'{PREFIX}/ext/added' +MAP_EXT_CHANGED = f'{PREFIX}/ext/changed' +MAP_EXT_REMOVED = f'{PREFIX}/ext/removed' + + +class MapNode(Node): + def __init__(self, name=None): + self.name = name if name else self.__class__.__name__ + super().__init__(self.name) + + # all map by name + self.data = {} + + # overwrite map in data if another map + # with name is received or log an error and ignore + # the new name + self.allow_overwrite = False + + # name of the id + self.id_name = 'name' + + # setup services for data manipulation + self.init_ros() + + def init_ros(self): + self.added_pub = self.create_publisher( + Map, MAP_ADDED, 10) + self.changed_pub = self.create_publisher( + Map, MAP_CHANGED, 10) + self.removed_pub = self.create_publisher( + String, MAP_REMOVED, 10) + self.marker_added_pub = self.create_publisher( + Marker, MAP_MARKER_ADDED, 10) + self.marker_changed_pub = self.create_publisher( + Marker, MAP_MARKER_CHANGED, 10) + self.marker_removed_pub = self.create_publisher( + String, MAP_MARKER_REMOVED, 10) + self.area_added_pub = self.create_publisher( + Area, MAP_AREA_ADDED, 10) + self.area_changed_pub = self.create_publisher( + Area, MAP_AREA_CHANGED, 10) + self.area_removed_pub = self.create_publisher( + String, MAP_AREA_REMOVED, 10) + self.wall_added_pub = self.create_publisher( + Wall, MAP_WALL_ADDED, 10) + self.wall_changed_pub = self.create_publisher( + Wall, MAP_WALL_CHANGED, 10) + self.wall_removed_pub = self.create_publisher( + String, MAP_WALL_REMOVED, 10) + self.path_added_pub = self.create_publisher( + Path, MAP_PATH_ADDED, 10) + self.path_changed_pub = self.create_publisher( + Path, MAP_PATH_CHANGED, 10) + self.path_removed_pub = self.create_publisher( + String, MAP_PATH_REMOVED, 10) + self.ext_added_pub = self.create_publisher( + External, MAP_EXT_ADDED, 10) + self.ext_changed_pub = self.create_publisher( + External, MAP_EXT_CHANGED, 10) + self.ext_removed_pub = self.create_publisher( + String, MAP_EXT_REMOVED, 10) + self.create_srv = self.create_service( + MapCreate, MAP_CREATE, + self.on_create) + self.delete_srv = self.create_service( + MapDelete, MAP_DELETE, + self.on_delete) + self.get_srv = self.create_service( + MapGet, MAP_GET, + self.on_get) + self.list_srv = self.create_service( + MapList, MAP_LIST, + self.on_list) + self.update_srv = self.create_service( + MapUpdate, MAP_UPDATE, + self.on_update) + self.overwrite_srv = self.create_service( + MapOverwrite, MAP_OVERWRITE, + self.on_overwrite) + self.create_marker_srv = self.create_service( + MapMarkerCreate, MAP_MARKER_CREATE, + self.on_attr_create('marker')) + self.delete_marker_srv = self.create_service( + MapMarkerDelete, MAP_MARKER_DELETE, + self.on_attr_delete('marker')) + self.list_marker_srv = self.create_service( + MapMarkerList, MAP_MARKER_LIST, + self.on_attr_list('marker')) + self.update_marker_srv = self.create_service( + MapMarkerUpdate, MAP_MARKER_UPDATE, + self.on_attr_update('marker')) + self.create_area_srv = self.create_service( + MapAreaCreate, MAP_AREA_CREATE, + self.on_attr_create('area')) + self.delete_area_srv = self.create_service( + MapAreaDelete, MAP_AREA_DELETE, + self.on_attr_delete('area')) + self.list_area_srv = self.create_service( + MapAreaList, MAP_AREA_LIST, + self.on_attr_list('area')) + self.update_area_srv = self.create_service( + MapAreaUpdate, MAP_AREA_UPDATE, + self.on_attr_update('area')) + self.create_wall_srv = self.create_service( + MapWallCreate, MAP_WALL_CREATE, + self.on_attr_create('wall')) + self.delete_wall_srv = self.create_service( + MapWallDelete, MAP_WALL_DELETE, + self.on_attr_delete('wall')) + self.list_wall_srv = self.create_service( + MapWallList, MAP_WALL_LIST, + self.on_attr_list('wall')) + self.update_wall_srv = self.create_service( + MapWallUpdate, MAP_WALL_UPDATE, + self.on_attr_update('wall')) + self.create_path_srv = self.create_service( + MapPathCreate, MAP_PATH_CREATE, + self.on_attr_create('path')) + self.delete_path_srv = self.create_service( + MapPathDelete, MAP_PATH_DELETE, + self.on_attr_delete('path')) + self.list_path_srv = self.create_service( + MapPathList, MAP_PATH_LIST, + self.on_attr_list('path')) + self.update_path_srv = self.create_service( + MapPathUpdate, MAP_PATH_UPDATE, + self.on_attr_update('path')) + self.create_ext_srv = self.create_service( + MapExtCreate, MAP_EXT_CREATE, + self.on_attr_create('ext')) + self.delete_ext_srv = self.create_service( + MapExtDelete, MAP_EXT_DELETE, + self.on_attr_delete('ext')) + self.list_ext_srv = self.create_service( + MapExtList, MAP_EXT_LIST, + self.on_attr_list('ext')) + self.update_ext_srv = self.create_service( + MapExtUpdate, MAP_EXT_UPDATE, + self.on_attr_update('ext')) + + def _get(self, _name): + if _name not in self.data: + return None + return self.data[_name] + + def on_attr_create(self, attr: str): + def _create(request, response): + _name = request.name + elem = self._get(_name) + if not elem: + self.get_logger().error( + f'Can not get map "{_name}"') + response.success = False + return response + getattr(elem, attr).append(request.item) + getattr(self, f'{attr}_added_pub').publish(request.item) + response.success = True + return response + return _create + + def on_attr_list(self, attr: str): + def _list(request, response): + _name = request.name + elem = self._get(_name) + data = getattr(elem, attr, []) + setattr(response, attr, data) + return response + return _list + + def on_attr_update(self, attr: str): + def _update(request, response): + _name = request.name + elem = self._get(_name) + idx = request.index + getattr(elem, attr)[idx] = request.item + getattr(self, f'{attr}_changed_pub').publish(request.item) + response.success = True + return response + return _update + + def on_attr_delete(self, attr: str): + def _delete(request, response): + _name = request.name + elem = self._get(_name) + idx = request.index + item = getattr(elem, attr)[idx] + getattr(elem, attr).remove(idx) + getattr(self, f'{attr}_removed_pub').publish(item) + return response + return _delete + + def on_create(self, request, response): + elem = request.map + _name = elem.name + if self._get(request.name) and not self.allow_overwrite: + self.get_logger().error( + 'Can not create, name ' + f'{_name} already exists.') + response.success = False + return response + self.data[_name] = elem + self.added_pub.publish(elem) + response.success = True + return response + + def on_delete(self, request, response): + _name = request.name + if not self._get(_name): + self.get_logger().error( + 'Can not delete, name ' + f'{_name} does not exist.') + response.success = False + return response + del self.data[_name] + self.removed_pub.publish(_name) + response.success = True + return response + + def on_get(self, request, response): + _name = request.name + elem = self._get(_name) + if elem: + response.map = elem + return response + else: + self.get_logger().info( + 'Mission ID ' + f'{request.name} does not exist!') + return response + + def on_list(self, request, response): + response.map = list(self.data.values()) + return response + + def on_update(self, request, response): + elem = request.map + _name = elem.name + if not self._get(request.name): + self.get_logger().error( + 'Can not update, name: ' + f'{_name} does not exist.') + response.success = False + return response + self.data[_name] = elem + self.changed_pub.publish(elem) + response.success = True + return response + + def on_overwrite(self, request, response): + """allow overwrite map in self.data""" + self.allow_overwrite = request.allow_overwrite + return response + + +def main(args=None): + rclpy.init(args=args) + + node = MapNode() + + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/lib/mapdesc_ros/mapdesc_ros/mapdesc_ros/node.py b/lib/mapdesc_ros/mapdesc_ros/mapdesc_ros/node.py new file mode 100644 index 0000000..3b0e7c8 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/mapdesc_ros/node.py @@ -0,0 +1,40 @@ +from pathlib import Path + +import rclpy + +from mapdesc.load.yaml import load_yaml + +from .convert_data import map_ros_to_mapdesc +from .map_desc import MapNode + + +class MapDescNode(MapNode): + def __init__(self): + super().__init__('mapdesc_node') + self.declare_parameter('map_yaml', '') + self.load_yaml_file() + + def load_yaml_file(self): + """Load yaml files and set as data""" + map_yaml = str(self.get_parameter('map_yaml').value) + if not map_yaml or map_yaml == '': + self.get_logger().warning('Map not set!') + return + if not Path(map_yaml).exists(): + self.get_logger().warning(f'Map does not exist: {map_yaml}') + return + map_mapdesc = load_yaml(map_yaml) + map_msg = map_ros_to_mapdesc(map_mapdesc) + self.data[map_msg.name] = map_msg + self.get_logger().info(f'Loaded map: {map_msg.name}') + + +def main(args=None): + rclpy.init(args=args) + node = MapDescNode() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/lib/mapdesc_ros/mapdesc_ros/package.xml b/lib/mapdesc_ros/mapdesc_ros/package.xml new file mode 100644 index 0000000..0f88db2 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/package.xml @@ -0,0 +1,21 @@ + + + + mapdesc_ros + 0.0.0 + ROS 2 wrapper for the map description package + abresser + BSD-3 + + rclpy + geometry_msgs + std_msgs + + ament_copyright + ament_flake8 + python3-pytest + + + ament_python + + diff --git a/lib/mapdesc_ros/mapdesc_ros/pytest.ini b/lib/mapdesc_ros/mapdesc_ros/pytest.ini new file mode 100644 index 0000000..791ce85 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/pytest.ini @@ -0,0 +1,6 @@ +[pytest] +# Set testpaths, otherwise pytest finds 'tests' in the examples directory +testpaths = test +# Add arguments for launch tests +addopts = --launch-args dut_arg:=test +junit_family=xunit2 \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_ros/resource/mapdesc_ros b/lib/mapdesc_ros/mapdesc_ros/resource/mapdesc_ros new file mode 100644 index 0000000..e69de29 diff --git a/lib/mapdesc_ros/mapdesc_ros/setup.cfg b/lib/mapdesc_ros/mapdesc_ros/setup.cfg new file mode 100644 index 0000000..1003583 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/setup.cfg @@ -0,0 +1,8 @@ +[develop] +script_dir=$base/lib/mapdesc_ros +[install] +install_scripts=$base/lib/mapdesc_ros +[coverage:run] +# This will let coverage find files with 0% coverage (not hit by tests at all) +source = . +omit = setup.py \ No newline at end of file diff --git a/lib/mapdesc_ros/mapdesc_ros/setup.py b/lib/mapdesc_ros/mapdesc_ros/setup.py new file mode 100644 index 0000000..f73ba44 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/setup.py @@ -0,0 +1,33 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'mapdesc_ros' + + +setup( + name=package_name, + version='0.0.1', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ( + os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*.launch.py'))), + ], + install_requires=['setuptools', 'mapdesc'], + requires=['mapdesc'], + zip_safe=True, + maintainer='abresser', + maintainer_email='Andreas.Bresser@dfki.de', + description='ROS 2 wrapper for the map description', + license='BSD-3', + tests_require=['pytest', 'pytest-cov'], + entry_points={ + 'console_scripts': [ + 'mapdesc_service = mapdesc_ros.node:main', + ], + } +) diff --git a/lib/mapdesc_ros/mapdesc_ros/test/test_copyright.py b/lib/mapdesc_ros/mapdesc_ros/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 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(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/lib/mapdesc_ros/mapdesc_ros/test/test_flake8.py b/lib/mapdesc_ros/mapdesc_ros/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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 ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/lib/mapdesc_ros/mapdesc_ros/test/test_mapdesc_service.py b/lib/mapdesc_ros/mapdesc_ros/test/test_mapdesc_service.py new file mode 100644 index 0000000..34e4137 --- /dev/null +++ b/lib/mapdesc_ros/mapdesc_ros/test/test_mapdesc_service.py @@ -0,0 +1,3 @@ +def test_get_marker(): + """Make sure the map we get from the ROS service has some marker.""" + assert 1 == 1 diff --git a/lib/mapdesc_ros/roscrud.yml b/lib/mapdesc_ros/roscrud.yml new file mode 100644 index 0000000..bdf18c5 --- /dev/null +++ b/lib/mapdesc_ros/roscrud.yml @@ -0,0 +1,15 @@ +# ROSCrud is a Tool to generate ROS 2 services to Create Read Update and Delete +# data, insprired by RESTful web services +# see https://en.wikipedia.org/wiki/Create,_read,_update_and_delete for details +- name: map + class_name: Map + id_name: name # equals the file name + package_name: mapdesc_ros + package_msgs_name: mapdesc_msgs + attributes: + - marker + - area + - wall + - path + - ext + # TODO: lanes - create getter/setter to set lane_nodes and lane_edges directly \ No newline at end of file diff --git a/lib/mapdesc_ros/run_tests.bash b/lib/mapdesc_ros/run_tests.bash new file mode 100755 index 0000000..5bcd8e8 --- /dev/null +++ b/lib/mapdesc_ros/run_tests.bash @@ -0,0 +1,13 @@ +#!/bin/bash +# run unit and integration tests INSIDE the docker container +source ./install/setup.bash +# TODO: move unit tests to an own folder +echo "" +echo "-- starting unit tests using colcon --" +echo "" +colcon test --pytest-with-coverage --event-handlers console_cohesion+ --return-code-on-test-failure +echo "" +echo "-- starting integration tests using launch_test --" +echo "" +# Note that you need to define each test individually +launch_test src/mapdesc_ros/integration_tests/launch_testing/marker_launch_test.py \ No newline at end of file diff --git a/lib/mapdesc_ros/test_docker.bash b/lib/mapdesc_ros/test_docker.bash new file mode 100755 index 0000000..d915a97 --- /dev/null +++ b/lib/mapdesc_ros/test_docker.bash @@ -0,0 +1,4 @@ +# run tests on local machine (no gitlab ci, but in a docker container) +docker compose build ros_with_pip +docker compose build mapdesc_ros +docker compose run mapdesc_ros \ No newline at end of file