├── .clang-format ├── .devcontainer ├── Dockerfile ├── devcontainer.json └── docker-compose.yml ├── .github └── workflows │ └── build_test.yaml ├── .gitignore ├── LICENSE ├── README.md ├── explore ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ ├── params.yaml │ └── params_costmap.yaml ├── doc │ ├── architecture.dia │ ├── screenshot.png │ └── wiki_doc.txt ├── include │ └── explore │ │ ├── costmap_client.h │ │ ├── costmap_tools.h │ │ ├── explore.h │ │ └── frontier_search.h ├── launch │ └── explore.launch.py ├── package.xml ├── src │ ├── costmap_client.cpp │ ├── explore.cpp │ └── frontier_search.cpp └── test │ └── test_explore.cpp └── map_merge ├── CHANGELOG.rst ├── CMakeLists.txt ├── config └── params.yaml ├── doc ├── architecture.dia ├── screenshot.jpg └── wiki_doc.txt ├── include ├── combine_grids │ ├── grid_compositor.h │ ├── grid_warper.h │ └── merging_pipeline.h └── map_merge │ ├── map_merge.h │ └── ros1_names.hpp ├── launch ├── from_map_server.launch.py ├── map_merge.launch.py ├── map_merge.rviz └── tb3_simulation │ ├── bringup_launch.py │ ├── config │ ├── nav2_multirobot_params_1.yaml │ └── nav2_multirobot_params_2.yaml │ ├── multi_tb3_simulation_launch.py │ ├── slam_toolbox.py │ ├── tb3_simulation_launch.py │ └── worlds │ └── world_only.model ├── package.xml ├── src ├── combine_grids │ ├── estimation_internal.h │ ├── grid_compositor.cpp │ ├── grid_warper.cpp │ └── merging_pipeline.cpp └── map_merge.cpp └── test ├── download.sh ├── download_data.sh ├── test_merging_pipeline.cpp └── testing_helpers.h /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakTemplateDeclarations: true 13 | AlwaysBreakBeforeMultilineStrings: false 14 | BreakBeforeBinaryOperators: false 15 | BreakBeforeTernaryOperators: false 16 | BreakConstructorInitializersBeforeComma: true 17 | BinPackParameters: true 18 | ColumnLimit: 80 19 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 20 | DerivePointerBinding: true 21 | ExperimentalAutoDetectBinPacking: false 22 | IndentCaseLabels: true 23 | MaxEmptyLinesToKeep: 1 24 | NamespaceIndentation: None 25 | ObjCSpaceBeforeProtocolList: true 26 | PenaltyBreakBeforeFirstCallParameter: 19 27 | PenaltyBreakComment: 60 28 | PenaltyBreakString: 1 29 | PenaltyBreakFirstLessLess: 1000 30 | PenaltyExcessCharacter: 100 31 | PenaltyReturnTypeOnItsOwnLine: 90 32 | PointerBindsToType: false 33 | SpacesBeforeTrailingComments: 2 34 | Cpp11BracedListStyle: true 35 | Standard: Cpp11 36 | IndentWidth: 2 37 | TabWidth: 2 38 | UseTab: Never 39 | BreakBeforeBraces: Linux 40 | IndentFunctionDeclarationAfterType: false 41 | SpacesInParentheses: false 42 | SpacesInAngles: false 43 | SpaceInEmptyParentheses: false 44 | SpacesInCStyleCastParentheses: false 45 | SpaceAfterControlStatementKeyword: true 46 | SpaceBeforeAssignmentOperators: true 47 | ContinuationIndentWidth: 4 48 | ... 49 | -------------------------------------------------------------------------------- /.devcontainer/Dockerfile: -------------------------------------------------------------------------------- 1 | # FROM nvidia/cuda:11.7.1-devel-ubuntu22.04 2 | FROM ubuntu:jammy 3 | # FROM nvidia/cuda:11.1.1-cudnn8-devel-ubuntu20.04 4 | # FROM ubuntu:focal 5 | 6 | ENV DEBIAN_FRONTEND noninteractive 7 | 8 | RUN apt-get update && apt-get install --no-install-recommends -y \ 9 | apt-utils \ 10 | bzip2 \ 11 | lbzip2 \ 12 | tar \ 13 | wget \ 14 | libzbar0 \ 15 | unzip \ 16 | build-essential \ 17 | zlib1g-dev \ 18 | libcurl4-gnutls-dev \ 19 | locales \ 20 | curl \ 21 | gnupg2 \ 22 | lsb-release \ 23 | ca-certificates \ 24 | && apt autoremove -y && apt clean -y \ 25 | && rm -rf /var/lib/apt/lists/* 26 | 27 | # https://index.ros.org/doc/ros2/Installation/Crystal/Linux-Install-Debians/ 28 | ENV ROS_DISTRO=humble 29 | 30 | ENV LANG=C.UTF-8 31 | ENV LC_ALL=C.UTF-8 32 | RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \ 33 | && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null 34 | 35 | RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' \ 36 | && wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - \ 37 | && apt update && apt install --no-install-recommends -y \ 38 | ros-${ROS_DISTRO}-ros-base \ 39 | ros-${ROS_DISTRO}-rclcpp-cascade-lifecycle \ 40 | ros-${ROS_DISTRO}-geographic-msgs \ 41 | ros-${ROS_DISTRO}-camera-info-manager \ 42 | ros-${ROS_DISTRO}-launch-testing-ament-cmake \ 43 | ros-${ROS_DISTRO}-diagnostic-updater \ 44 | ros-${ROS_DISTRO}-rviz2 \ 45 | ros-${ROS_DISTRO}-gazebo-ros \ 46 | ros-${ROS_DISTRO}-gazebo-ros-pkgs \ 47 | ros-${ROS_DISTRO}-gazebo-msgs \ 48 | ros-${ROS_DISTRO}-gazebo-plugins \ 49 | ros-${ROS_DISTRO}-robot-state-publisher \ 50 | ros-${ROS_DISTRO}-cv-bridge \ 51 | ros-${ROS_DISTRO}-message-filters \ 52 | ros-${ROS_DISTRO}-image-transport \ 53 | ros-${ROS_DISTRO}-rqt* \ 54 | ros-${ROS_DISTRO}-slam-toolbox \ 55 | ros-${ROS_DISTRO}-navigation2 \ 56 | ros-${ROS_DISTRO}-nav2-bringup \ 57 | ros-${ROS_DISTRO}-behaviortree-cpp-v3 \ 58 | ros-${ROS_DISTRO}-angles \ 59 | ros-${ROS_DISTRO}-ompl \ 60 | ros-${ROS_DISTRO}-turtlebot3* \ 61 | ros-${ROS_DISTRO}-image-geometry \ 62 | && apt autoremove && apt clean -y \ 63 | && rm -rf /var/lib/apt/lists/* 64 | 65 | # Install ROS2 gazebo dependencies 66 | RUN apt update && apt-get install --no-install-recommends -y \ 67 | libglvnd0 \ 68 | libglx0 \ 69 | libegl1 \ 70 | libxext6 \ 71 | libx11-6 \ 72 | libblkid-dev \ 73 | e2fslibs-dev \ 74 | libboost-all-dev \ 75 | libaudit-dev \ 76 | git \ 77 | nano \ 78 | # ------------------------------ 79 | && apt autoremove && apt clean -y \ 80 | && rm -rf /var/lib/apt/lists/* 81 | 82 | RUN apt update && apt install --no-install-recommends -y \ 83 | python3-dev \ 84 | python3-pip \ 85 | python3-colcon-common-extensions \ 86 | && pip3 install rosdep \ 87 | && rosdep init \ 88 | && rosdep update \ 89 | && apt autoremove && apt clean -y \ 90 | && rm -rf /var/lib/apt/lists/* 91 | 92 | # Or your actual UID, GID on Linux if not the default 1000 93 | ARG USERNAME=dev 94 | ARG USER_UID=1000 95 | ARG USER_GID=$USER_UID 96 | 97 | # Create a non-root user to use if preferred - see https://aka.ms/vscode-remote/containers/non-root-user. 98 | RUN groupadd --gid $USER_GID $USERNAME \ 99 | && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \ 100 | # Add sudo support for non-root user 101 | && apt-get update && apt-get install -y sudo \ 102 | && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ 103 | && chmod 0440 /etc/sudoers.d/$USERNAME \ 104 | && apt-get autoremove && apt-get clean -y \ 105 | && rm -rf /var/lib/apt/lists/* 106 | 107 | RUN \ 108 | mkdir -p /home/${USERNAME}/.ignition/fuel/ \ 109 | && echo "servers:\n -\n name: osrf\n url: https://api.ignitionrobotics.org" >> /home/${USERNAME}/.ignition/fuel/config.yaml \ 110 | && chown ${USERNAME} /home/${USERNAME}/.ignition \ 111 | && GAZEBO_SOURCE="source /usr/share/gazebo/setup.sh" \ 112 | && echo $GAZEBO_SOURCE >> "/home/${USERNAME}/.bashrc" \ 113 | && chown ${USERNAME} /home/${USERNAME}/.ignition 114 | 115 | # ROS2 source setup 116 | RUN ROS_SOURCE="source /opt/ros/${ROS_DISTRO}/setup.sh" \ 117 | && echo $ROS_SOURCE >> "/home/${USERNAME}/.bashrc" 118 | 119 | WORKDIR /workspace/ros_ws/src 120 | # Give permission to non-root user to access the workspace 121 | RUN chown -R ${USERNAME} /workspace/ros_ws 122 | RUN git clone https://github.com/charlielito/slam_gmapping.git --branch feature/namespace_launch 123 | 124 | ENV TURTLEBOT3_MODEL=waffle 125 | ENV GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models 126 | 127 | # Switch back to dialog for any ad-hoc use of apt-get 128 | ENV DEBIAN_FRONTEND= 129 | CMD ["/bin/bash"] -------------------------------------------------------------------------------- /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Dev container", 3 | "dockerComposeFile": "docker-compose.yml", 4 | "service": "m-explore-ros2-humble", 5 | "workspaceFolder": "/workspace/ros_ws", 6 | "runArgs": [ 7 | "--runtime=nvidia" 8 | ], 9 | "settings": { 10 | "terminal.integrated.automationShell.linux": "/bin/bash", 11 | }, 12 | "extensions": [ 13 | "ms-azuretools.vscode-docker", 14 | "eamodio.gitlens", 15 | "streetsidesoftware.code-spell-checker", 16 | "oderwat.indent-rainbow", 17 | "ms-vsliveshare.vsliveshare", 18 | "pkief.material-icon-theme", 19 | "ms-python.python", 20 | "ms-vscode.cpptools", 21 | "xaver.clang-format", 22 | "ms-python.vscode-pylance", 23 | ], 24 | } -------------------------------------------------------------------------------- /.devcontainer/docker-compose.yml: -------------------------------------------------------------------------------- 1 | # version: "3.7" 2 | version: "2.3" 3 | services: 4 | m-explore-ros2-humble: 5 | runtime: nvidia 6 | build: 7 | context: ../ 8 | dockerfile: .devcontainer/Dockerfile 9 | working_dir: /workspace/ros_ws 10 | user: dev 11 | 12 | network_mode: host 13 | ports: 14 | - "80:80" 15 | expose: 16 | - 80 17 | 18 | init: true 19 | privileged: true 20 | 21 | environment: 22 | - DISPLAY=$DISPLAY 23 | - QT_X11_NO_MITSHM=1 24 | - UDEV=1 25 | - NVIDIA_DRIVER_CAPABILITIES=compute,utility,display 26 | volumes: 27 | # Update this to wherever you want VS Code to mount the folder of your project 28 | - ..:/workspace/ros_ws/src/m-explore-ros2 29 | # Forwards the local Docker socket to the container. 30 | - /var/run/docker.sock:/var/run/docker.sock 31 | # Enable GUI environments 32 | - /tmp/.X11-unix:/tmp/.X11-unix:rw 33 | 34 | devices: 35 | - /dev/bus/usb:/dev/bus/usb 36 | # NVIDIA drivers to use OpenGL, etc... 37 | - /dev/nvidia0:/dev/nvidia0 38 | - /dev/nvidiactl:/dev/nvidiactl 39 | - /dev/nvidia-uvm:/dev/nvidia-uvm 40 | - /dev/input:/dev/input 41 | - /dev/snd:/dev/snd 42 | 43 | # Uncomment the next four lines if you will use a ptrace-based debuggers like C++, Go, and Rust. 44 | cap_add: 45 | - SYS_PTRACE 46 | security_opt: 47 | - seccomp:unconfined 48 | 49 | # Overrides default command so things don't shut down after the process ends. 50 | stdin_open: true 51 | tty: true 52 | 53 | command: "/bin/bash" 54 | -------------------------------------------------------------------------------- /.github/workflows/build_test.yaml: -------------------------------------------------------------------------------- 1 | name: ROS2 build/tests 2 | on: 3 | pull_request: 4 | jobs: 5 | build_and_tests: 6 | runs-on: ubuntu-20.04 7 | strategy: 8 | matrix: 9 | ros_distribution: 10 | - foxy 11 | - galactic 12 | - humble 13 | include: 14 | # Foxy Fitzroy (June 2020 - May 2023) 15 | - docker_image: ubuntu:focal 16 | ros_distribution: foxy 17 | # Galactic Geochelone (May 2021 - November 2022) 18 | - docker_image: ubuntu:focal 19 | ros_distribution: galactic 20 | # Humble Hawksbill (May 2022 - May 2027) 21 | - docker_image: ubuntu:jammy 22 | ros_distribution: humble 23 | container: 24 | image: ${{ matrix.docker_image }} 25 | steps: 26 | - name: setup ROS environment 27 | uses: ros-tooling/setup-ros@v0.4 28 | with: 29 | required-ros-distributions: ${{ matrix.ros_distribution }} 30 | - name: Checkout repository 31 | uses: actions/checkout@v3 32 | - name: build multirobot_map_merge and explore_lite 33 | uses: ros-tooling/action-ros-ci@v0.2 34 | with: 35 | package-name: multirobot_map_merge explore_lite 36 | target-ros2-distro: ${{ matrix.ros_distribution }} 37 | skip-tests: true 38 | - name: Run gtests manually multirobot_map_merge 39 | run: | 40 | . /opt/ros/${{ matrix.ros_distribution }}/setup.sh && . ros_ws/install/setup.sh 41 | cd ros_ws/build/multirobot_map_merge 42 | ./test_merging_pipeline 43 | - name: Run gtests manually explore_lite 44 | run: | 45 | . /opt/ros/${{ matrix.ros_distribution }}/setup.sh && . ros_ws/install/setup.sh 46 | cd ros_ws/build/explore_lite 47 | ./test_explore -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *sublime-* 2 | *.svg 3 | *.xcf 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2015-2016, Carlos Alvarez. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of the Carlos Alvarez nor the names of its 17 | contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # m-explore ROS2 port 2 | 3 | ROS2 package port for multi-robot autonomous exploration of [m-explore](https://github.com/hrnr/m-explore). Currently tested on Eloquent, Dashing, Foxy, and Galactic distros. 4 | 5 | ### Contents 6 | 1. [Autonomous exploration](#Autonomous-exploration) 7 | * [Demo in simulation with a TB3 robot](#Simulation-with-a-TB3-robot) 8 | * [Demo with a JetBot](#On-a-JetBot-with-realsense-cameras) 9 | * [Instructions for the simulation demo](#Running-the-explore-demo-with-TB3) 10 | 2. [Multirobot map merge](#Multirobot-map-merge) 11 | * [Simulation demo with known initial poses](#Known-initial-poses) 12 | * [Simulation demo with unknown initial poses](#Unknown-initial-poses) 13 | * [ROS2 requirements](#ROS2-requirements) 14 | * [Instructions for simulation demos](#Running-the-demo-with-TB3) 15 | 16 | ## Autonomous exploration 17 | 18 | ### Simulation with a TB3 robot 19 | https://user-images.githubusercontent.com/8033598/128805356-be90a880-16c6-4fc9-8f54-e3302873dc8c.mp4 20 | 21 | 22 | ### On a JetBot with realsense cameras 23 | https://user-images.githubusercontent.com/18732666/128493567-6841dde0-2250-4d81-9bcb-8b216e0fb34d.mp4 24 | 25 | 26 | Installing 27 | ---------- 28 | 29 | No binaries yet. 30 | 31 | Building 32 | -------- 33 | 34 | Build as a standard colcon package. There are no special dependencies needed 35 | (use rosdep to resolve dependencies in ROS). 36 | 37 | RUNNING 38 | ------- 39 | To run with a params file just run it with 40 | ``` 41 | ros2 run explore_lite explore --ros-args --params-file /m-explore-ros2/explore/config/params.yaml 42 | ``` 43 | 44 | ### Running the explore demo with TB3 45 | Install nav2 and tb3 simulation. You can follow the [tutorial](https://navigation.ros.org/getting_started/index.html#installation). 46 | 47 | Then just run the nav2 stack with slam: 48 | 49 | ``` 50 | export TURTLEBOT3_MODEL=waffle 51 | export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models 52 | ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True 53 | ``` 54 | 55 | And run this package with 56 | ``` 57 | ros2 launch explore_lite explore.launch.py 58 | ``` 59 | 60 | You can open rviz2 and add the exploration frontiers marker (topic is `explore/frontiers`) to see the algorithm working and the frontier chosen to explore. 61 | 62 | ### Additional features 63 | #### Stop/Resume exploration 64 | By default the exploration node will start right away the frontier-based exploration algorithm. Alternatively, you can stop the exploration by publishing to a `False` to `explore/resume` topic. This will stop the exploration and the robot will stop moving. You can resume the exploration by publishing to `True` to `explore/resume`. 65 | 66 | #### Returning to initial pose 67 | The robot will return to its initial pose after exploration if you want by defining the parameter `return_to_init` to `True` when launching the node. 68 | 69 | #### TB3 troubleshooting (with foxy) 70 | If you have trouble with TB3 in simulation, as we did, add these extra steps for configuring it. 71 | 72 | ``` 73 | source /opt/ros/${ROS_DISTRO}/setup.bash 74 | export TURTLEBOT3_MODEL=waffle 75 | sudo rm -rf /opt/ros/${ROS_DISTRO}/share/turtlebot3_simulations 76 | sudo git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations /opt/ros/${ROS_DISTRO}/share/turtlebot3_simulations 77 | export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_simulations/turtlebot3_gazebo/models 78 | ``` 79 | 80 | Then you'll be able to run it. 81 | 82 | ______________________________________________________________________ 83 | ## Multirobot map merge 84 | 85 | This package works with known and unknown initial poses of the robots. It merges the maps of the robots and publishes the merged map. Some results in simulation are shown below. 86 | 87 | ### Known initial poses 88 | 89 | This modality gives normally the best results. The original ROS1 code only supports [slam_gmapping](https://github.com/ros-perception/slam_gmapping) type of maps for the merge. The following shows the result with that. 90 | 91 | https://user-images.githubusercontent.com/8033598/144522712-c31fb4bb-bb5a-4859-b3e1-8ad665f80696.mp4 92 | 93 | We also support using [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) in a yet [experimental branch](https://github.com/robo-friends/m-explore-ros2/tree/feature/slam_toolbox_compat). The following demo shows the map merging using the currently supported and most used ROS2-SLAM library. 94 | 95 | https://user-images.githubusercontent.com/8033598/170846935-cfae9f3f-5edd-43ea-b993-7b3ba1db921b.mp4 96 | 97 | 98 | ### Unknown initial poses 99 | It works better if the robots start very close (< 3 meters) to each other so their relative positions can be calculated properly. 100 | 101 | https://user-images.githubusercontent.com/8033598/144522696-517d54fd-74d0-4c55-9aca-f1b9679afb3e.mp4 102 | 103 | ### ROS2 requirements 104 | 105 | #### SLAM 106 | Because of the logic that merges the maps, currently as a straightforward port to ROS2 from the ROS1 version, the SLAM needs to be done using the ROS1 defacto slam option which is [slam_gmapping](https://github.com/ros-perception/slam_gmapping), which hasn't been ported officially to ROS2 yet. There is an unofficial port but it lacks to pass a namespace to its launch file. For that, this repo was tested with one of the authors of this package's [fork](https://github.com/charlielito/slam_gmapping/tree/feature/namespace_launch). You'll need to git clone to your workspace and build it with colcon. 107 | 108 | 109 | ``` 110 | cd 111 | git clone https://github.com/charlielito/slam_gmapping.git --branch feature/namespace_launch 112 | cd .. 113 | colcon build --symlink-install --packages-up-to slam_gmapping 114 | ``` 115 | 116 | **Note**: You could use [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) instead but you need to use this [experimental branch](https://github.com/robo-friends/m-explore-ros2/tree/feature/slam_toolbox_compat) which is still under development. 117 | 118 | #### Nav2 gazebo spawner (deprecated in humble) 119 | To spawn multiple robots, you need the `nav2_gazebo_spawner` which does not come up with the `nav2-bringup` installation. For that, install it with `sudo apt install ros-${ROS_DISTRO}-nav2-gazebo-spawner`. 120 | Note that was the case for release previous to `humble` but since `humble` release, this package is deprecated and a gazebo node is used for this. So, if you are using `humble` or newer, you don't need to install it. 121 | 122 | #### Nav2 config files 123 | This repo has some config examples and launch files for running this package with 2 TB3 robots and a world with nav2. Nonetheless, they are only compatible with the galactic/humble distros and since some breaking changes were introduced in this distro, if you want to try it with another ros2 distro you'll need to tweak those param files for that nav2's distro version (which shouldn't be hard). 124 | 125 | ### Running the demo with TB3 126 | First, you'll need to launch the whole simulation stack, nav2 stacks and slam stacks per robot. For that just launch:: 127 | ``` 128 | export TURTLEBOT3_MODEL=waffle 129 | export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models 130 | ros2 launch multirobot_map_merge multi_tb3_simulation_launch.py slam_gmapping:=True 131 | ``` 132 | Now run the merging node: 133 | ``` 134 | ros2 launch multirobot_map_merge map_merge.launch.py 135 | ``` 136 | 137 | By default, the demo runs with known initial poses. You can change that by launching again both launch commands with the flag `known_init_poses:=False` 138 | 139 | Then you can start moving each robot with its corresponding rviz2 interface by sending nav2 goals. To see the map merged just launch rviz2: 140 | ``` 141 | rviz2 -d /src/m-explore-ros2/map_merge/launch/map_merge.rviz 142 | ``` 143 | 144 | **Note**: If you want to use slam_toolbox, launch `multirobot_map_merge` with the following flag instead: `slam_toolbox:=True`. Remember to use the experimental branch mentioned above. 145 | 146 | WIKI 147 | ---- 148 | No wiki yet. 149 | 150 | COPYRIGHT 151 | --------- 152 | 153 | Packages are licensed under BSD license. See respective files for details. 154 | -------------------------------------------------------------------------------- /explore/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package explore_lite 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2021-08-01) 6 | ------------------ 7 | * First working port in ros2 foxy 8 | * Contributors: Carlos Alvarez, Juan Gavlis -------------------------------------------------------------------------------- /explore/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(explore_lite) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # Set flag depending on distro 19 | if(NOT DEFINED ENV{ROS_DISTRO}) 20 | message(FATAL_ERROR "ROS_DISTRO is not defined." ) 21 | endif() 22 | if("$ENV{ROS_DISTRO}" STREQUAL "eloquent") 23 | message(STATUS "Build for ROS2 eloquent") 24 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DELOQUENT") 25 | elseif("$ENV{ROS_DISTRO}" STREQUAL "dashing") 26 | message(STATUS "Build for ROS2 dashing") 27 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDASHING") 28 | else() 29 | message(STATUS "BuilD for ROS2: " "$ENV{ROS_DISTRO}") 30 | endif() 31 | 32 | # find dependencies 33 | find_package(ament_cmake REQUIRED) 34 | find_package(rclcpp REQUIRED) 35 | find_package(std_msgs REQUIRED) 36 | find_package(sensor_msgs REQUIRED) 37 | find_package(tf2_ros REQUIRED) 38 | find_package(tf2 REQUIRED) 39 | find_package(tf2_geometry_msgs REQUIRED) 40 | find_package(nav2_msgs REQUIRED) 41 | find_package(nav_msgs REQUIRED) 42 | find_package(map_msgs REQUIRED) 43 | find_package(visualization_msgs REQUIRED) 44 | find_package(nav2_costmap_2d REQUIRED) 45 | 46 | 47 | set(DEPENDENCIES 48 | rclcpp 49 | std_msgs 50 | sensor_msgs 51 | tf2 52 | tf2_ros 53 | tf2_geometry_msgs 54 | nav2_msgs 55 | nav_msgs 56 | map_msgs 57 | nav2_costmap_2d 58 | visualization_msgs 59 | ) 60 | 61 | include_directories( 62 | include 63 | ) 64 | 65 | install( 66 | DIRECTORY include/explore/ 67 | DESTINATION include/explore/ 68 | ) 69 | 70 | install(DIRECTORY 71 | config 72 | DESTINATION share/${PROJECT_NAME} 73 | ) 74 | install(DIRECTORY 75 | launch 76 | DESTINATION share/${PROJECT_NAME} 77 | ) 78 | 79 | 80 | add_executable(explore 81 | src/costmap_client.cpp 82 | src/explore.cpp 83 | src/frontier_search.cpp 84 | ) 85 | 86 | target_include_directories(explore PUBLIC 87 | $ 88 | $) 89 | 90 | 91 | target_link_libraries(explore ${rclcpp_LIBRARIES}) 92 | 93 | ament_target_dependencies(explore ${DEPENDENCIES}) 94 | 95 | install(TARGETS explore 96 | DESTINATION lib/${PROJECT_NAME}) 97 | 98 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 99 | 100 | ############# 101 | ## Testing ## 102 | ############# 103 | if(BUILD_TESTING) 104 | find_package(ament_lint_auto REQUIRED) 105 | # the following line skips the linter which checks for copyrights 106 | set(ament_cmake_copyright_FOUND TRUE) 107 | set(ament_cmake_cpplint_FOUND TRUE) 108 | ament_lint_auto_find_test_dependencies() 109 | 110 | find_package(ament_cmake_gtest REQUIRED) 111 | 112 | ament_add_gtest(test_explore test/test_explore.cpp) 113 | target_link_libraries(test_explore ${catkin_LIBRARIES}) 114 | ament_target_dependencies(test_explore ${DEPENDENCIES}) 115 | 116 | 117 | endif() 118 | 119 | 120 | ament_export_include_directories(include) 121 | ament_package() -------------------------------------------------------------------------------- /explore/config/params.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | robot_base_frame: base_link 4 | return_to_init: true 5 | costmap_topic: map 6 | costmap_updates_topic: map_updates 7 | visualize: true 8 | planner_frequency: 0.15 9 | progress_timeout: 30.0 10 | potential_scale: 3.0 11 | orientation_scale: 0.0 12 | gain_scale: 1.0 13 | transform_tolerance: 0.3 14 | min_frontier_size: 0.75 -------------------------------------------------------------------------------- /explore/config/params_costmap.yaml: -------------------------------------------------------------------------------- 1 | explore_node: 2 | ros__parameters: 3 | robot_base_frame: base_link 4 | costmap_topic: /global_costmap/costmap 5 | costmap_updates_topic: /global_costmap/costmap_updates 6 | visualize: true 7 | planner_frequency: 0.15 8 | progress_timeout: 30.0 9 | potential_scale: 3.0 10 | orientation_scale: 0.0 11 | gain_scale: 1.0 12 | transform_tolerance: 0.3 13 | min_frontier_size: 0.5 -------------------------------------------------------------------------------- /explore/doc/architecture.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robo-friends/m-explore-ros2/9c121aebe52d28f3846a1cbb6ebb017cc85f9e53/explore/doc/architecture.dia -------------------------------------------------------------------------------- /explore/doc/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robo-friends/m-explore-ros2/9c121aebe52d28f3846a1cbb6ebb017cc85f9e53/explore/doc/screenshot.png -------------------------------------------------------------------------------- /explore/doc/wiki_doc.txt: -------------------------------------------------------------------------------- 1 | <> 2 | 3 | <> 4 | 5 | <> 6 | 7 | == Overview == 8 | This package provides greedy frontier-based exploration. When node is running, robot will greedily explore its environment until no frontiers could be found. Movement commands will be send to [[move_base]]. 9 | 10 | {{attachment:screenshot.png||width="755px"}} 11 | 12 | Unlike similar packages, {{{explore_lite}}} does not create its own costmap, which makes it easier to configure and more efficient (lighter on resources). Node simply subscribes to <> messages. Commands for robot movement are send to [[move_base]] node. 13 | 14 | Node can do frontier filtering and can operate even on non-inflated maps. Goal blacklisting allows to deal with places inaccessible for robot. 15 | 16 | <> 17 | 18 | == Architecture == 19 | {{{explore_lite}}} uses [[move_base]] for navigation. You need to run properly configured [[move_base]] node. 20 | 21 | {{attachment:architecture.svg||width="755px"}} 22 | 23 | {{{explore_lite}}} subscribes to a <> and <> messages to construct a map where it looks for frontiers. You can either use costmap published by [[move_base]] (ie. `/global_costmap/costmap`) or you can use map constructed by mapping algorithm (SLAM). 24 | 25 | Depending on your environment you may achieve better results with either SLAM map or costmap published by `move_base`. Advantage of `move_base` costmap is the inflation which helps to deal with some very small unexplorable frontiers. When you are using a raw map produced by SLAM you should set the `min_frontier_size` parameter to some reasonable number to deal with the small frontiers. For details on both setups check the `explore.launch` and `explore_costmap.launch` launch files. 26 | 27 | == Setup == 28 | 29 | Before starting experimenting with {{{explore_lite}}} you need to have working [[move_base]] for navigation. You should be able to navigate with [[move_base]] manually through [[rviz]]. Please refer to [[navigation#Tutorials]] for setting up [[move_base]] and the rest of the navigation stack with your robot. 30 | 31 | You should be also able to to navigate with [[move_base]] though unknown space in the map. If you set the goal to unknown place in the map, planning and navigating should work. With most planners this should work by default, refer to [[navfn#Parameters]] if you need to setup this for [[navfn]] planner (but should be enabled by default). Navigation through unknown space is required for {{{explore_lite}}}. 32 | 33 | If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`. 34 | 35 | If you have [[move_base]] configured correctly, you can start experimenting with {{{explore_lite}}}. Provided `explore.launch` should work out-of-the box in most cases, but as always you might need to adjust topic names and frame names according to your setup. 36 | 37 | == ROS API == 38 | {{{ 39 | #!clearsilver CS/NodeAPI 40 | 41 | name = explore 42 | desc = Provides exploration services offered by this package. Exploration will start immediately after node initialization. 43 | 44 | pub { 45 | 0.name = ~frontiers 46 | 0.type = visualization_msgs/MarkerArray 47 | 0.desc = Visualization of frontiers considered by exploring algorithm. Each frontier is visualized by frontier points in blue and with a small sphere, which visualize the cost of the frontiers (costlier frontiers will have smaller spheres). 48 | } 49 | sub { 50 | 0.name = costmap 51 | 0.type = nav_msgs/OccupancyGrid 52 | 0.desc = Map which will be used for exploration planning. Can be either costmap from [[move_base]] or map created by SLAM (see above). Occupancy grid must have got properly marked unknown space, mapping algorithms usually track unknown space by default. If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`. 53 | 54 | 1.name = costmap_updates 55 | 1.type = map_msgs/OccupancyGridUpdate 56 | 1.desc = Incremental updates on costmap. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic. 57 | } 58 | 59 | param { 60 | 0.name = ~robot_base_frame 61 | 0.default = `base_link` 62 | 0.type = string 63 | 0.desc = The name of the base frame of the robot. This is used for determining robot position on map. Mandatory. 64 | 65 | 1.name = ~costmap_topic 66 | 1.default = `costmap` 67 | 1.type = string 68 | 1.desc = Specifies topic of source <>. Mandatory. 69 | 70 | 3.name = ~costmap_updates_topic 71 | 3.default = `costmap_updates` 72 | 3.type = string 73 | 3.desc = Specifies topic of source <>. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic. 74 | 75 | 4.name = ~visualize 76 | 4.default = `false` 77 | 4.type = bool 78 | 4.desc = Specifies whether or not publish visualized frontiers. 79 | 80 | 6.name = ~planner_frequency 81 | 6.default = `1.0` 82 | 6.type = double 83 | 6.desc = Rate in Hz at which new frontiers will computed and goal reconsidered. 84 | 85 | 7.name = ~progress_timeout 86 | 7.default = `30.0` 87 | 7.type = double 88 | 7.desc = Time in seconds. When robot do not make any progress for `progress_timeout`, current goal will be abandoned. 89 | 90 | 8.name = ~potential_scale 91 | 8.default = `1e-3` 92 | 8.type = double 93 | 8.desc = Used for weighting frontiers. This multiplicative parameter affects frontier potential component of the frontier weight (distance to frontier). 94 | 95 | 9.name = ~orientation_scale 96 | 9.default = `0` 97 | 9.type = double 98 | 9.desc = Used for weighting frontiers. This multiplicative parameter affects frontier orientation component of the frontier weight. This parameter does currently nothing and is provided solely for forward compatibility. 99 | 100 | 10.name = ~gain_scale 101 | 10.default = `1.0` 102 | 10.type = double 103 | 10.desc = Used for weighting frontiers. This multiplicative parameter affects frontier gain component of the frontier weight (frontier size). 104 | 105 | 11.name = ~transform_tolerance 106 | 11.default = `0.3` 107 | 11.type = double 108 | 11.desc = Transform tolerance to use when transforming robot pose. 109 | 110 | 12.name = ~min_frontier_size 111 | 12.default = `0.5` 112 | 12.type = double 113 | 12.desc = Minimum size of the frontier to consider the frontier as the exploration goal. In meters. 114 | } 115 | 116 | req_tf { 117 | 0.from = global_frame 118 | 0.to = robot_base_frame 119 | 0.desc = This transformation is usually provided by mapping algorithm. Those frames are usually called `map` and `base_link`. For adjusting `robot_base_frame` name see respective parameter. You don't need to set `global_frame`. The name for `global_frame` will be sourced from `costmap_topic` automatically. 120 | } 121 | 122 | act_called { 123 | 0.name = move_base 124 | 0.type = move_base_msgs/MoveBaseAction 125 | 0.desc = [[move_base]] actionlib API for posting goals. See [[move_base#Action API]] for details. This expects [[move_base]] node in the same namespace as `explore_lite`, you may want to remap this node if this is not true. 126 | } 127 | }}} 128 | 129 | == Acknowledgements == 130 | 131 | This package was developed as part of my bachelor thesis at [[http://www.mff.cuni.cz/to.en/|Charles University]] in Prague. 132 | 133 | {{{ 134 | @masterthesis{Hörner2016, 135 | author = {Jiří Hörner}, 136 | title = {Map-merging for multi-robot system}, 137 | address = {Prague}, 138 | year = {2016}, 139 | school = {Charles University in Prague, Faculty of Mathematics and Physics}, 140 | type = {Bachelor's thesis}, 141 | URL = {https://is.cuni.cz/webapps/zzp/detail/174125/}, 142 | } 143 | }}} 144 | 145 | This project was initially based on [[explore]] package by Charles !DuHadway. Most of the node has been rewritten since then. The current frontier search algorithm is based on [[frontier_exploration]] by Paul Bovbel. 146 | 147 | ## AUTOGENERATED DON'T DELETE 148 | ## CategoryPackage 149 | -------------------------------------------------------------------------------- /explore/include/explore/costmap_client.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * Copyright (c) 2021, Carlos Alvarez, Juan Galvis. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #ifndef COSTMAP_CLIENT_ 39 | #define COSTMAP_CLIENT_ 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include "nav2_costmap_2d/costmap_2d_ros.hpp" 51 | 52 | namespace explore 53 | { 54 | class Costmap2DClient 55 | { 56 | public: 57 | /** 58 | * @brief Contructs client and start listening 59 | * @details Constructor will block until first map update is received and 60 | * map is ready to use, also will block before trasformation 61 | * robot_base_frame <-> global_frame is available. 62 | * 63 | * @param node node handle to retrieve parameters from 64 | * @param tf_listener Will be used for transformation of robot pose. 65 | */ 66 | Costmap2DClient(rclcpp::Node& node, const tf2_ros::Buffer* tf_listener); 67 | /** 68 | * @brief Get the pose of the robot in the global frame of the costmap 69 | * @return pose of the robot in the global frame of the costmap 70 | */ 71 | geometry_msgs::msg::Pose getRobotPose() const; 72 | 73 | /** 74 | * @brief Return a pointer to the "master" costmap which receives updates from 75 | * all the layers. 76 | * 77 | * This pointer will stay the same for the lifetime of Costmap2DClient object. 78 | */ 79 | nav2_costmap_2d::Costmap2D* getCostmap() 80 | { 81 | return &costmap_; 82 | } 83 | 84 | /** 85 | * @brief Return a pointer to the "master" costmap which receives updates from 86 | * all the layers. 87 | * 88 | * This pointer will stay the same for the lifetime of Costmap2DClient object. 89 | */ 90 | const nav2_costmap_2d::Costmap2D* getCostmap() const 91 | { 92 | return &costmap_; 93 | } 94 | 95 | /** 96 | * @brief Returns the global frame of the costmap 97 | * @return The global frame of the costmap 98 | */ 99 | const std::string& getGlobalFrameID() const 100 | { 101 | return global_frame_; 102 | } 103 | 104 | /** 105 | * @brief Returns the local frame of the costmap 106 | * @return The local frame of the costmap 107 | */ 108 | const std::string& getBaseFrameID() const 109 | { 110 | return robot_base_frame_; 111 | } 112 | 113 | protected: 114 | void updateFullMap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); 115 | void updatePartialMap(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg); 116 | 117 | nav2_costmap_2d::Costmap2D costmap_; 118 | bool costmap_received_ = false; ///< @brief Flag indicating whether costmap 119 | ///< callback has been called 120 | 121 | const tf2_ros::Buffer* const tf_; ///< @brief Used for transforming 122 | /// point clouds 123 | rclcpp::Node& node_; 124 | std::string global_frame_; ///< @brief The global frame for the costmap 125 | std::string robot_base_frame_; ///< @brief The frame_id of the robot base 126 | double transform_tolerance_; ///< timeout before transform errors 127 | 128 | private: 129 | // will be unsubscribed at destruction 130 | rclcpp::Subscription::SharedPtr costmap_sub_; 131 | rclcpp::Subscription::SharedPtr 132 | costmap_updates_sub_; 133 | }; 134 | 135 | } // namespace explore 136 | 137 | #endif 138 | -------------------------------------------------------------------------------- /explore/include/explore/costmap_tools.h: -------------------------------------------------------------------------------- 1 | #ifndef COSTMAP_TOOLS_H_ 2 | #define COSTMAP_TOOLS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "nav2_costmap_2d/costmap_2d_ros.hpp" 9 | 10 | namespace frontier_exploration 11 | { 12 | /** 13 | * @brief Determine 4-connected neighbourhood of an input cell, checking for map 14 | * edges 15 | * @param idx input cell index 16 | * @param costmap Reference to map data 17 | * @return neighbour cell indexes 18 | */ 19 | std::vector nhood4(unsigned int idx, 20 | const nav2_costmap_2d::Costmap2D& costmap) 21 | { 22 | // get 4-connected neighbourhood indexes, check for edge of map 23 | std::vector out; 24 | 25 | unsigned int size_x_ = costmap.getSizeInCellsX(), 26 | size_y_ = costmap.getSizeInCellsY(); 27 | 28 | if (idx > size_x_ * size_y_ - 1) { 29 | RCLCPP_WARN(rclcpp::get_logger("FrontierExploration"), "Evaluating nhood " 30 | "for offmap point"); 31 | return out; 32 | } 33 | 34 | if (idx % size_x_ > 0) { 35 | out.push_back(idx - 1); 36 | } 37 | if (idx % size_x_ < size_x_ - 1) { 38 | out.push_back(idx + 1); 39 | } 40 | if (idx >= size_x_) { 41 | out.push_back(idx - size_x_); 42 | } 43 | if (idx < size_x_ * (size_y_ - 1)) { 44 | out.push_back(idx + size_x_); 45 | } 46 | return out; 47 | } 48 | 49 | /** 50 | * @brief Determine 8-connected neighbourhood of an input cell, checking for map 51 | * edges 52 | * @param idx input cell index 53 | * @param costmap Reference to map data 54 | * @return neighbour cell indexes 55 | */ 56 | std::vector nhood8(unsigned int idx, 57 | const nav2_costmap_2d::Costmap2D& costmap) 58 | { 59 | // get 8-connected neighbourhood indexes, check for edge of map 60 | std::vector out = nhood4(idx, costmap); 61 | 62 | unsigned int size_x_ = costmap.getSizeInCellsX(), 63 | size_y_ = costmap.getSizeInCellsY(); 64 | 65 | if (idx > size_x_ * size_y_ - 1) { 66 | return out; 67 | } 68 | 69 | if (idx % size_x_ > 0 && idx >= size_x_) { 70 | out.push_back(idx - 1 - size_x_); 71 | } 72 | if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) { 73 | out.push_back(idx - 1 + size_x_); 74 | } 75 | if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) { 76 | out.push_back(idx + 1 - size_x_); 77 | } 78 | if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) { 79 | out.push_back(idx + 1 + size_x_); 80 | } 81 | 82 | return out; 83 | } 84 | 85 | /** 86 | * @brief Find nearest cell of a specified value 87 | * @param result Index of located cell 88 | * @param start Index initial cell to search from 89 | * @param val Specified value to search for 90 | * @param costmap Reference to map data 91 | * @return True if a cell with the requested value was found 92 | */ 93 | bool nearestCell(unsigned int& result, unsigned int start, unsigned char val, 94 | const nav2_costmap_2d::Costmap2D& costmap) 95 | { 96 | const unsigned char* map = costmap.getCharMap(); 97 | const unsigned int size_x = costmap.getSizeInCellsX(), 98 | size_y = costmap.getSizeInCellsY(); 99 | 100 | if (start >= size_x * size_y) { 101 | return false; 102 | } 103 | 104 | // initialize breadth first search 105 | std::queue bfs; 106 | std::vector visited_flag(size_x * size_y, false); 107 | 108 | // push initial cell 109 | bfs.push(start); 110 | visited_flag[start] = true; 111 | 112 | // search for neighbouring cell matching value 113 | while (!bfs.empty()) { 114 | unsigned int idx = bfs.front(); 115 | bfs.pop(); 116 | 117 | // return if cell of correct value is found 118 | if (map[idx] == val) { 119 | result = idx; 120 | return true; 121 | } 122 | 123 | // iterate over all adjacent unvisited cells 124 | for (unsigned nbr : nhood8(idx, costmap)) { 125 | if (!visited_flag[nbr]) { 126 | bfs.push(nbr); 127 | visited_flag[nbr] = true; 128 | } 129 | } 130 | } 131 | 132 | return false; 133 | } 134 | } // namespace frontier_exploration 135 | #endif 136 | -------------------------------------------------------------------------------- /explore/include/explore/explore.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Robert Bosch LLC. 6 | * Copyright (c) 2015-2016, Jiri Horner. 7 | * Copyright (c) 2021, Carlos Alvarez, Juan Galvis. 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the Jiri Horner nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | *********************************************************************/ 38 | #ifndef NAV_EXPLORE_H_ 39 | #define NAV_EXPLORE_H_ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | 58 | #include "nav2_msgs/action/navigate_to_pose.hpp" 59 | #include "rclcpp_action/rclcpp_action.hpp" 60 | 61 | using namespace std::placeholders; 62 | #ifdef ELOQUENT 63 | #define ACTION_NAME "NavigateToPose" 64 | #elif DASHING 65 | #define ACTION_NAME "NavigateToPose" 66 | #else 67 | #define ACTION_NAME "navigate_to_pose" 68 | #endif 69 | namespace explore 70 | { 71 | /** 72 | * @class Explore 73 | * @brief A class adhering to the robot_actions::Action interface that moves the 74 | * robot base to explore its environment. 75 | */ 76 | class Explore : public rclcpp::Node 77 | { 78 | public: 79 | Explore(); 80 | ~Explore(); 81 | 82 | void start(); 83 | void stop(bool finished_exploring = false); 84 | void resume(); 85 | 86 | using NavigationGoalHandle = 87 | rclcpp_action::ClientGoalHandle; 88 | 89 | private: 90 | /** 91 | * @brief Make a global plan 92 | */ 93 | void makePlan(); 94 | 95 | // /** 96 | // * @brief Publish a frontiers as markers 97 | // */ 98 | void visualizeFrontiers( 99 | const std::vector& frontiers); 100 | 101 | bool goalOnBlacklist(const geometry_msgs::msg::Point& goal); 102 | 103 | NavigationGoalHandle::SharedPtr navigation_goal_handle_; 104 | // void 105 | // goal_response_callback(std::shared_future 106 | // future); 107 | void reachedGoal(const NavigationGoalHandle::WrappedResult& result, 108 | const geometry_msgs::msg::Point& frontier_goal); 109 | 110 | rclcpp::Publisher::SharedPtr 111 | marker_array_publisher_; 112 | rclcpp::Logger logger_ = rclcpp::get_logger("ExploreNode"); 113 | tf2_ros::Buffer tf_buffer_; 114 | tf2_ros::TransformListener tf_listener_; 115 | 116 | Costmap2DClient costmap_client_; 117 | rclcpp_action::Client::SharedPtr 118 | move_base_client_; 119 | frontier_exploration::FrontierSearch search_; 120 | rclcpp::TimerBase::SharedPtr exploring_timer_; 121 | // rclcpp::TimerBase::SharedPtr oneshot_; 122 | 123 | rclcpp::Subscription::SharedPtr resume_subscription_; 124 | void resumeCallback(const std_msgs::msg::Bool::SharedPtr msg); 125 | 126 | std::vector frontier_blacklist_; 127 | geometry_msgs::msg::Point prev_goal_; 128 | double prev_distance_; 129 | rclcpp::Time last_progress_; 130 | size_t last_markers_count_; 131 | 132 | geometry_msgs::msg::Pose initial_pose_; 133 | void returnToInitialPose(void); 134 | 135 | // parameters 136 | double planner_frequency_; 137 | double potential_scale_, orientation_scale_, gain_scale_; 138 | double progress_timeout_; 139 | bool visualize_; 140 | bool return_to_init_; 141 | std::string robot_base_frame_; 142 | bool resuming_ = false; 143 | }; 144 | } // namespace explore 145 | 146 | #endif 147 | -------------------------------------------------------------------------------- /explore/include/explore/frontier_search.h: -------------------------------------------------------------------------------- 1 | #ifndef FRONTIER_SEARCH_H_ 2 | #define FRONTIER_SEARCH_H_ 3 | 4 | #include "nav2_costmap_2d/costmap_2d_ros.hpp" 5 | 6 | namespace frontier_exploration 7 | { 8 | /** 9 | * @brief Represents a frontier 10 | * 11 | */ 12 | struct Frontier { 13 | std::uint32_t size; 14 | double min_distance; 15 | double cost; 16 | geometry_msgs::msg::Point initial; 17 | geometry_msgs::msg::Point centroid; 18 | geometry_msgs::msg::Point middle; 19 | std::vector points; 20 | }; 21 | 22 | /** 23 | * @brief Thread-safe implementation of a frontier-search task for an input 24 | * costmap. 25 | */ 26 | class FrontierSearch 27 | { 28 | public: 29 | FrontierSearch() 30 | { 31 | } 32 | 33 | /** 34 | * @brief Constructor for search task 35 | * @param costmap Reference to costmap data to search. 36 | */ 37 | FrontierSearch(nav2_costmap_2d::Costmap2D* costmap, double potential_scale, 38 | double gain_scale, double min_frontier_size); 39 | 40 | /** 41 | * @brief Runs search implementation, outward from the start position 42 | * @param position Initial position to search from 43 | * @return List of frontiers, if any 44 | */ 45 | std::vector searchFrom(geometry_msgs::msg::Point position); 46 | 47 | protected: 48 | /** 49 | * @brief Starting from an initial cell, build a frontier from valid adjacent 50 | * cells 51 | * @param initial_cell Index of cell to start frontier building 52 | * @param reference Reference index to calculate position from 53 | * @param frontier_flag Flag vector indicating which cells are already marked 54 | * as frontiers 55 | * @return new frontier 56 | */ 57 | Frontier buildNewFrontier(unsigned int initial_cell, unsigned int reference, 58 | std::vector& frontier_flag); 59 | 60 | /** 61 | * @brief isNewFrontierCell Evaluate if candidate cell is a valid candidate 62 | * for a new frontier. 63 | * @param idx Index of candidate cell 64 | * @param frontier_flag Flag vector indicating which cells are already marked 65 | * as frontiers 66 | * @return true if the cell is frontier cell 67 | */ 68 | bool isNewFrontierCell(unsigned int idx, 69 | const std::vector& frontier_flag); 70 | 71 | /** 72 | * @brief computes frontier cost 73 | * @details cost function is defined by potential_scale and gain_scale 74 | * 75 | * @param frontier frontier for which compute the cost 76 | * @return cost of the frontier 77 | */ 78 | double frontierCost(const Frontier& frontier); 79 | 80 | private: 81 | nav2_costmap_2d::Costmap2D* costmap_; 82 | unsigned char* map_; 83 | unsigned int size_x_, size_y_; 84 | double potential_scale_, gain_scale_; 85 | double min_frontier_size_; 86 | }; 87 | } // namespace frontier_exploration 88 | #endif 89 | -------------------------------------------------------------------------------- /explore/launch/explore.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch_ros.actions import Node 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import DeclareLaunchArgument 8 | from launch.substitutions import LaunchConfiguration 9 | 10 | 11 | def generate_launch_description(): 12 | ld = LaunchDescription() 13 | config = os.path.join( 14 | get_package_share_directory("explore_lite"), "config", "params.yaml" 15 | ) 16 | use_sim_time = LaunchConfiguration("use_sim_time") 17 | namespace = LaunchConfiguration("namespace") 18 | 19 | declare_use_sim_time_argument = DeclareLaunchArgument( 20 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 21 | ) 22 | declare_namespace_argument = DeclareLaunchArgument( 23 | "namespace", 24 | default_value="", 25 | description="Namespace for the explore node", 26 | ) 27 | 28 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 29 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 30 | # https://github.com/ros/geometry2/issues/32 31 | # https://github.com/ros/robot_state_publisher/pull/30 32 | remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] 33 | 34 | node = Node( 35 | package="explore_lite", 36 | name="explore_node", 37 | namespace=namespace, 38 | executable="explore", 39 | parameters=[config, {"use_sim_time": use_sim_time}], 40 | output="screen", 41 | remappings=remappings, 42 | ) 43 | ld.add_action(declare_use_sim_time_argument) 44 | ld.add_action(declare_namespace_argument) 45 | ld.add_action(node) 46 | return ld 47 | -------------------------------------------------------------------------------- /explore/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | explore_lite 5 | 1.0.0 6 | 7 | Lightweight frontier-based exploration ROS2 port. 8 | 9 | Carlos Alvarez 10 | Carlos Alvarez 11 | BSD 12 | 13 | ament_cmake 14 | 15 | ament_lint_auto 16 | ament_lint_common 17 | ament_cmake 18 | map_msgs 19 | nav2_costmap_2d 20 | nav2_msgs 21 | nav_msgs 22 | rclcpp 23 | sensor_msgs 24 | std_msgs 25 | tf2 26 | tf2_geometry_msgs 27 | tf2_ros 28 | visualization_msgs 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /explore/src/costmap_client.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * Copyright (c) 2021, Carlos Alvarez, Juan Galvis. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | namespace explore 46 | { 47 | // static translation table to speed things up 48 | std::array init_translation_table(); 49 | static const std::array cost_translation_table__ = 50 | init_translation_table(); 51 | 52 | Costmap2DClient::Costmap2DClient(rclcpp::Node& node, const tf2_ros::Buffer* tf) 53 | : tf_(tf), node_(node) 54 | { 55 | std::string costmap_topic; 56 | std::string costmap_updates_topic; 57 | 58 | node_.declare_parameter("costmap_topic", std::string("costmap")); 59 | node_.declare_parameter("costmap_updates_topic", 60 | std::string("costmap_updates")); 61 | node_.declare_parameter("robot_base_frame", std::string("base_" 62 | "link")); 63 | // transform tolerance is used for all tf transforms here 64 | node_.declare_parameter("transform_tolerance", 0.3); 65 | 66 | node_.get_parameter("costmap_topic", costmap_topic); 67 | node_.get_parameter("costmap_updates_topic", costmap_updates_topic); 68 | node_.get_parameter("robot_base_frame", robot_base_frame_); 69 | node_.get_parameter("transform_tolerance", transform_tolerance_); 70 | 71 | /* initialize costmap */ 72 | costmap_sub_ = node_.create_subscription( 73 | costmap_topic, 1000, 74 | [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { 75 | costmap_received_ = true; 76 | updateFullMap(msg); 77 | }); 78 | 79 | // ROS1 CODE 80 | // auto costmap_msg = 81 | // ros::topic::waitForMessage( 82 | // costmap_topic, subscription_nh); 83 | 84 | // Spin some until the callback gets called to replicate 85 | // ros::topic::waitForMessage 86 | RCLCPP_INFO(node_.get_logger(), 87 | "Waiting for costmap to become available, topic: %s", 88 | costmap_topic.c_str()); 89 | while (!costmap_received_) { 90 | rclcpp::spin_some(node_.get_node_base_interface()); 91 | // Wait for a second 92 | usleep(1000000); 93 | } 94 | // updateFullMap(costmap_msg); // this is already called in the callback of 95 | // the costmap_sub_ 96 | 97 | /* subscribe to map updates */ 98 | costmap_updates_sub_ = 99 | node_.create_subscription( 100 | costmap_updates_topic, 1000, 101 | [this](const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg) { 102 | updatePartialMap(msg); 103 | }); 104 | 105 | // ROS1 CODE. 106 | // TODO: Do we need this? 107 | /* resolve tf prefix for robot_base_frame */ 108 | // std::string tf_prefix = tf::getPrefixParam(node_); 109 | // robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_); 110 | 111 | // we need to make sure that the transform between the robot base frame and 112 | // the global frame is available 113 | 114 | // the global frame is set in the costmap callback. This is why we need to 115 | // ensure that a costmap is received 116 | 117 | /* tf transform is necessary for getRobotPose */ 118 | auto last_error = node_.now(); 119 | std::string tf_error; 120 | while (rclcpp::ok() && 121 | !tf_->canTransform(global_frame_, robot_base_frame_, 122 | tf2::TimePointZero, tf2::durationFromSec(0.1), 123 | &tf_error)) { 124 | rclcpp::spin_some(node_.get_node_base_interface()); 125 | if (last_error + tf2::durationFromSec(5.0) < node_.now()) { 126 | RCLCPP_WARN(node_.get_logger(), 127 | "Timed out waiting for transform from %s to %s to become " 128 | "available " 129 | "before subscribing to costmap, tf error: %s", 130 | robot_base_frame_.c_str(), global_frame_.c_str(), 131 | tf_error.c_str()); 132 | last_error = node_.now(); 133 | ; 134 | } 135 | // The error string will accumulate and errors will typically be the same, 136 | // so the last 137 | // will do for the warning above. Reset the string here to avoid 138 | // accumulation. 139 | tf_error.clear(); 140 | } 141 | } 142 | 143 | void Costmap2DClient::updateFullMap( 144 | const nav_msgs::msg::OccupancyGrid::SharedPtr msg) 145 | { 146 | global_frame_ = msg->header.frame_id; 147 | 148 | unsigned int size_in_cells_x = msg->info.width; 149 | unsigned int size_in_cells_y = msg->info.height; 150 | double resolution = msg->info.resolution; 151 | double origin_x = msg->info.origin.position.x; 152 | double origin_y = msg->info.origin.position.y; 153 | 154 | RCLCPP_DEBUG(node_.get_logger(), "received full new map, resizing to: %d, %d", 155 | size_in_cells_x, size_in_cells_y); 156 | costmap_.resizeMap(size_in_cells_x, size_in_cells_y, resolution, origin_x, 157 | origin_y); 158 | 159 | // lock as we are accessing raw underlying map 160 | auto* mutex = costmap_.getMutex(); 161 | std::lock_guard lock(*mutex); 162 | 163 | // fill map with data 164 | unsigned char* costmap_data = costmap_.getCharMap(); 165 | size_t costmap_size = costmap_.getSizeInCellsX() * costmap_.getSizeInCellsY(); 166 | RCLCPP_DEBUG(node_.get_logger(), "full map update, %lu values", costmap_size); 167 | for (size_t i = 0; i < costmap_size && i < msg->data.size(); ++i) { 168 | unsigned char cell_cost = static_cast(msg->data[i]); 169 | costmap_data[i] = cost_translation_table__[cell_cost]; 170 | } 171 | RCLCPP_DEBUG(node_.get_logger(), "map updated, written %lu values", 172 | costmap_size); 173 | } 174 | 175 | void Costmap2DClient::updatePartialMap( 176 | const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg) 177 | { 178 | RCLCPP_DEBUG(node_.get_logger(), "received partial map update"); 179 | global_frame_ = msg->header.frame_id; 180 | 181 | if (msg->x < 0 || msg->y < 0) { 182 | RCLCPP_DEBUG(node_.get_logger(), 183 | "negative coordinates, invalid update. x: %d, y: %d", msg->x, 184 | msg->y); 185 | return; 186 | } 187 | 188 | size_t x0 = static_cast(msg->x); 189 | size_t y0 = static_cast(msg->y); 190 | size_t xn = msg->width + x0; 191 | size_t yn = msg->height + y0; 192 | 193 | // lock as we are accessing raw underlying map 194 | auto* mutex = costmap_.getMutex(); 195 | std::lock_guard lock(*mutex); 196 | 197 | size_t costmap_xn = costmap_.getSizeInCellsX(); 198 | size_t costmap_yn = costmap_.getSizeInCellsY(); 199 | 200 | if (xn > costmap_xn || x0 > costmap_xn || yn > costmap_yn || 201 | y0 > costmap_yn) { 202 | RCLCPP_WARN(node_.get_logger(), 203 | "received update doesn't fully fit into existing map, " 204 | "only part will be copied. received: [%lu, %lu], [%lu, %lu] " 205 | "map is: [0, %lu], [0, %lu]", 206 | x0, xn, y0, yn, costmap_xn, costmap_yn); 207 | } 208 | 209 | // update map with data 210 | unsigned char* costmap_data = costmap_.getCharMap(); 211 | size_t i = 0; 212 | for (size_t y = y0; y < yn && y < costmap_yn; ++y) { 213 | for (size_t x = x0; x < xn && x < costmap_xn; ++x) { 214 | size_t idx = costmap_.getIndex(x, y); 215 | unsigned char cell_cost = static_cast(msg->data[i]); 216 | costmap_data[idx] = cost_translation_table__[cell_cost]; 217 | ++i; 218 | } 219 | } 220 | } 221 | 222 | geometry_msgs::msg::Pose Costmap2DClient::getRobotPose() const 223 | { 224 | geometry_msgs::msg::PoseStamped robot_pose; 225 | geometry_msgs::msg::Pose empty_pose; 226 | robot_pose.header.frame_id = robot_base_frame_; 227 | robot_pose.header.stamp = node_.now(); 228 | 229 | auto& clk = *node_.get_clock(); 230 | 231 | // get the global pose of the robot 232 | try { 233 | robot_pose = tf_->transform(robot_pose, global_frame_, 234 | tf2::durationFromSec(transform_tolerance_)); 235 | } catch (tf2::LookupException& ex) { 236 | RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, 237 | "No Transform available Error looking up robot pose: " 238 | "%s\n", 239 | ex.what()); 240 | return empty_pose; 241 | } catch (tf2::ConnectivityException& ex) { 242 | RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, 243 | "Connectivity Error looking up robot pose: %s\n", 244 | ex.what()); 245 | return empty_pose; 246 | } catch (tf2::ExtrapolationException& ex) { 247 | RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, 248 | "Extrapolation Error looking up robot pose: %s\n", 249 | ex.what()); 250 | return empty_pose; 251 | } catch (tf2::TransformException& ex) { 252 | RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, "Other error: %s\n", 253 | ex.what()); 254 | return empty_pose; 255 | } 256 | 257 | return robot_pose.pose; 258 | } 259 | 260 | std::array init_translation_table() 261 | { 262 | std::array cost_translation_table; 263 | 264 | // lineary mapped from [0..100] to [0..255] 265 | for (size_t i = 0; i < 256; ++i) { 266 | cost_translation_table[i] = 267 | static_cast(1 + (251 * (i - 1)) / 97); 268 | } 269 | 270 | // special values: 271 | cost_translation_table[0] = 0; // NO obstacle 272 | cost_translation_table[99] = 253; // INSCRIBED obstacle 273 | cost_translation_table[100] = 254; // LETHAL obstacle 274 | cost_translation_table[static_cast(-1)] = 255; // UNKNOWN 275 | 276 | return cost_translation_table; 277 | } 278 | 279 | } // namespace explore 280 | -------------------------------------------------------------------------------- /explore/src/frontier_search.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | #include "nav2_costmap_2d/cost_values.hpp" 8 | 9 | namespace frontier_exploration 10 | { 11 | using nav2_costmap_2d::FREE_SPACE; 12 | using nav2_costmap_2d::LETHAL_OBSTACLE; 13 | using nav2_costmap_2d::NO_INFORMATION; 14 | 15 | FrontierSearch::FrontierSearch(nav2_costmap_2d::Costmap2D* costmap, 16 | double potential_scale, double gain_scale, 17 | double min_frontier_size) 18 | : costmap_(costmap) 19 | , potential_scale_(potential_scale) 20 | , gain_scale_(gain_scale) 21 | , min_frontier_size_(min_frontier_size) 22 | { 23 | } 24 | 25 | std::vector 26 | FrontierSearch::searchFrom(geometry_msgs::msg::Point position) 27 | { 28 | std::vector frontier_list; 29 | 30 | // Sanity check that robot is inside costmap bounds before searching 31 | unsigned int mx, my; 32 | if (!costmap_->worldToMap(position.x, position.y, mx, my)) { 33 | RCLCPP_ERROR(rclcpp::get_logger("FrontierSearch"), "Robot out of costmap " 34 | "bounds, cannot search " 35 | "for frontiers"); 36 | return frontier_list; 37 | } 38 | 39 | // make sure map is consistent and locked for duration of search 40 | std::lock_guard lock( 41 | *(costmap_->getMutex())); 42 | 43 | map_ = costmap_->getCharMap(); 44 | size_x_ = costmap_->getSizeInCellsX(); 45 | size_y_ = costmap_->getSizeInCellsY(); 46 | 47 | // initialize flag arrays to keep track of visited and frontier cells 48 | std::vector frontier_flag(size_x_ * size_y_, false); 49 | std::vector visited_flag(size_x_ * size_y_, false); 50 | 51 | // initialize breadth first search 52 | std::queue bfs; 53 | 54 | // find closest clear cell to start search 55 | unsigned int clear, pos = costmap_->getIndex(mx, my); 56 | if (nearestCell(clear, pos, FREE_SPACE, *costmap_)) { 57 | bfs.push(clear); 58 | } else { 59 | bfs.push(pos); 60 | RCLCPP_WARN(rclcpp::get_logger("FrontierSearch"), "Could not find nearby " 61 | "clear cell to start " 62 | "search"); 63 | } 64 | visited_flag[bfs.front()] = true; 65 | 66 | while (!bfs.empty()) { 67 | unsigned int idx = bfs.front(); 68 | bfs.pop(); 69 | 70 | // iterate over 4-connected neighbourhood 71 | for (unsigned nbr : nhood4(idx, *costmap_)) { 72 | // add to queue all free, unvisited cells, use descending search in case 73 | // initialized on non-free cell 74 | if (map_[nbr] <= map_[idx] && !visited_flag[nbr]) { 75 | visited_flag[nbr] = true; 76 | bfs.push(nbr); 77 | // check if cell is new frontier cell (unvisited, NO_INFORMATION, free 78 | // neighbour) 79 | } else if (isNewFrontierCell(nbr, frontier_flag)) { 80 | frontier_flag[nbr] = true; 81 | Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag); 82 | if (new_frontier.size * costmap_->getResolution() >= 83 | min_frontier_size_) { 84 | frontier_list.push_back(new_frontier); 85 | } 86 | } 87 | } 88 | } 89 | 90 | // set costs of frontiers 91 | for (auto& frontier : frontier_list) { 92 | frontier.cost = frontierCost(frontier); 93 | } 94 | std::sort( 95 | frontier_list.begin(), frontier_list.end(), 96 | [](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; }); 97 | 98 | return frontier_list; 99 | } 100 | 101 | Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, 102 | unsigned int reference, 103 | std::vector& frontier_flag) 104 | { 105 | // initialize frontier structure 106 | Frontier output; 107 | output.centroid.x = 0; 108 | output.centroid.y = 0; 109 | output.size = 1; 110 | output.min_distance = std::numeric_limits::infinity(); 111 | 112 | // record initial contact point for frontier 113 | unsigned int ix, iy; 114 | costmap_->indexToCells(initial_cell, ix, iy); 115 | costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y); 116 | 117 | // push initial gridcell onto queue 118 | std::queue bfs; 119 | bfs.push(initial_cell); 120 | 121 | // cache reference position in world coords 122 | unsigned int rx, ry; 123 | double reference_x, reference_y; 124 | costmap_->indexToCells(reference, rx, ry); 125 | costmap_->mapToWorld(rx, ry, reference_x, reference_y); 126 | 127 | while (!bfs.empty()) { 128 | unsigned int idx = bfs.front(); 129 | bfs.pop(); 130 | 131 | // try adding cells in 8-connected neighborhood to frontier 132 | for (unsigned int nbr : nhood8(idx, *costmap_)) { 133 | // check if neighbour is a potential frontier cell 134 | if (isNewFrontierCell(nbr, frontier_flag)) { 135 | // mark cell as frontier 136 | frontier_flag[nbr] = true; 137 | unsigned int mx, my; 138 | double wx, wy; 139 | costmap_->indexToCells(nbr, mx, my); 140 | costmap_->mapToWorld(mx, my, wx, wy); 141 | 142 | geometry_msgs::msg::Point point; 143 | point.x = wx; 144 | point.y = wy; 145 | output.points.push_back(point); 146 | 147 | // update frontier size 148 | output.size++; 149 | 150 | // update centroid of frontier 151 | output.centroid.x += wx; 152 | output.centroid.y += wy; 153 | 154 | // determine frontier's distance from robot, going by closest gridcell 155 | // to robot 156 | double distance = sqrt(pow((double(reference_x) - double(wx)), 2.0) + 157 | pow((double(reference_y) - double(wy)), 2.0)); 158 | if (distance < output.min_distance) { 159 | output.min_distance = distance; 160 | output.middle.x = wx; 161 | output.middle.y = wy; 162 | } 163 | 164 | // add to queue for breadth first search 165 | bfs.push(nbr); 166 | } 167 | } 168 | } 169 | 170 | // average out frontier centroid 171 | output.centroid.x /= output.size; 172 | output.centroid.y /= output.size; 173 | return output; 174 | } 175 | 176 | bool FrontierSearch::isNewFrontierCell(unsigned int idx, 177 | const std::vector& frontier_flag) 178 | { 179 | // check that cell is unknown and not already marked as frontier 180 | if (map_[idx] != NO_INFORMATION || frontier_flag[idx]) { 181 | return false; 182 | } 183 | 184 | // frontier cells should have at least one cell in 4-connected neighbourhood 185 | // that is free 186 | for (unsigned int nbr : nhood4(idx, *costmap_)) { 187 | if (map_[nbr] == FREE_SPACE) { 188 | return true; 189 | } 190 | } 191 | 192 | return false; 193 | } 194 | 195 | double FrontierSearch::frontierCost(const Frontier& frontier) 196 | { 197 | return (potential_scale_ * frontier.min_distance * 198 | costmap_->getResolution()) - 199 | (gain_scale_ * frontier.size * costmap_->getResolution()); 200 | } 201 | } // namespace frontier_exploration 202 | -------------------------------------------------------------------------------- /explore/test/test_explore.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, Carlos Alvarez. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Carlos Alvarez nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #define private public 42 | 43 | inline static bool same_point(const geometry_msgs::msg::Point& one, 44 | const geometry_msgs::msg::Point& two) 45 | { 46 | double dx = one.x - two.x; 47 | double dy = one.y - two.y; 48 | double dist = sqrt(dx * dx + dy * dy); 49 | return dist < 0.01; 50 | } 51 | 52 | TEST(Explore, testSameGoal) 53 | { 54 | geometry_msgs::msg::Point goal1; 55 | geometry_msgs::msg::Point goal2; 56 | // Populate the goal with known values 57 | goal1.x = 1.0; 58 | goal1.y = 2.0; 59 | goal1.z = 3.0; 60 | 61 | goal2.x = 0.0; 62 | goal2.y = 0.0; 63 | goal2.z = 0.0; 64 | auto same_goal = same_point(goal1, goal2); 65 | EXPECT_FALSE(same_goal); 66 | goal2.x = goal1.x; 67 | goal2.y = goal1.y; 68 | goal2.z = goal1.z; 69 | same_goal = same_point(goal1, goal2); 70 | EXPECT_TRUE(same_goal); 71 | } 72 | 73 | int main(int argc, char** argv) 74 | { 75 | testing::InitGoogleTest(&argc, argv); 76 | return RUN_ALL_TESTS(); 77 | } 78 | -------------------------------------------------------------------------------- /map_merge/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package multirobot_map_merge 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.1.4 (2021-01-07) 6 | ------------------ 7 | * use C++14 8 | * support both OpenCV 3 and OpenCV 4 (support Debian Buster) 9 | * Contributors: Jiri Horner 10 | 11 | 2.1.3 (2021-01-03) 12 | ------------------ 13 | * add missing dependencies to catkin_package calls 14 | * update map_merge for OpenCV 4 15 | * Contributors: Jiri Horner 16 | 17 | 2.1.2 (2021-01-02) 18 | ------------------ 19 | * support for ROS Melodic 20 | * bugfix: zero resolution of the merged grid for known initial posiiton 21 | * bugfix: estimation_confidence parameter had no effect 22 | * map_merge: set origin of merged grid in its centre 23 | * map_merge: add new launch file 24 | * map_merge with 2 maps served by map_server 25 | * bugfix: ensure that we never output map with 0 resolution 26 | * bugfix: nullptr derefence while setting resolution of output grid 27 | * Contributors: Jiri Horner 28 | 29 | 2.1.1 (2017-12-16) 30 | ------------------ 31 | * fix bugs in CMakeLists.txt: install nodes in packages, so they get shipped in debian packages. fixes `#11 `_ 32 | * map_merge: add bibtex to wiki page 33 | * Contributors: Jiri Horner 34 | 35 | 2.1.0 (2017-10-30) 36 | ------------------ 37 | * no major changes. Released together with explore_lite. 38 | 39 | 2.0.0 (2017-03-26) 40 | ------------------ 41 | * map_merge: upgrade to package format 2 42 | * node completely rewritten based on my work included in opencv 43 | * uses more reliable features by default -> more robust merging 44 | * known_init_poses is now by default false to make it easy to start for new users 45 | * Contributors: Jiri Horner 46 | 47 | 1.0.1 (2017-03-25) 48 | ------------------ 49 | * map_merge: use inverted tranform 50 | * transform needs to be inverted before using 51 | * map_merge: change package description 52 | * we support merging with unknown initial positions 53 | * Contributors: Jiri Horner 54 | 55 | 1.0.0 (2016-05-11) 56 | ------------------ 57 | * initial release 58 | * Contributors: Jiri Horner 59 | -------------------------------------------------------------------------------- /map_merge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(multirobot_map_merge) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++17 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 17) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(geometry_msgs REQUIRED) 22 | find_package(image_geometry REQUIRED) 23 | find_package(map_msgs REQUIRED) 24 | find_package(nav_msgs REQUIRED) 25 | find_package(tf2_geometry_msgs REQUIRED) 26 | find_package(tf2 REQUIRED) 27 | find_package(tf2_ros REQUIRED) 28 | 29 | find_package(Boost REQUIRED COMPONENTS thread) 30 | 31 | # OpenCV is required for merging without initial positions 32 | find_package(OpenCV REQUIRED) 33 | if("${OpenCV_VERSION}" VERSION_LESS "3.0") 34 | message(FATAL_ERROR "This package needs OpenCV >= 3.0") 35 | endif() 36 | if("${OpenCV_VERSION}" VERSION_LESS "4.0") 37 | message(WARNING "This package supports OpenCV 3, but some features may not be " 38 | "available. Upgrade to OpenCV 4 to take advantage of all features.") 39 | endif() 40 | 41 | set(DEPENDENCIES 42 | rclcpp 43 | geometry_msgs 44 | image_geometry 45 | map_msgs 46 | nav_msgs 47 | tf2_geometry_msgs 48 | tf2 49 | tf2_ros 50 | OpenCV 51 | ) 52 | 53 | ## Specify additional locations of header files 54 | include_directories( 55 | # ${Boost_INCLUDE_DIRS} 56 | # ${OpenCV_INCLUDE_DIRS} 57 | include 58 | ) 59 | 60 | install( 61 | DIRECTORY include/map_merge/ 62 | DESTINATION include/map_merge/ 63 | ) 64 | 65 | install(DIRECTORY 66 | launch 67 | config 68 | DESTINATION share/${PROJECT_NAME} 69 | ) 70 | 71 | # we want static linking for now 72 | add_library(combine_grids STATIC 73 | src/combine_grids/grid_compositor.cpp 74 | src/combine_grids/grid_warper.cpp 75 | src/combine_grids/merging_pipeline.cpp 76 | ) 77 | 78 | add_executable(map_merge 79 | src/map_merge.cpp 80 | ) 81 | 82 | ############# 83 | ## Install ## 84 | ############# 85 | target_include_directories(map_merge PUBLIC 86 | $ 87 | $) 88 | target_include_directories(combine_grids PUBLIC 89 | "$" 90 | "$" 91 | ${rclcpp_INCLUDE_DIRS} 92 | ${OpenCV_INCLUDE_DIRS} 93 | ) 94 | 95 | target_link_libraries(combine_grids ${rclcpp_LIBRARIES} ${OpenCV_LIBS}) 96 | # target_link_libraries(combine_grids ${OpenCV_LIBRARIES}) 97 | 98 | target_link_libraries(map_merge combine_grids) 99 | # target_link_libraries(map_merge) 100 | 101 | ament_target_dependencies(map_merge ${DEPENDENCIES}) 102 | ament_target_dependencies(combine_grids ${DEPENDENCIES}) 103 | 104 | install( 105 | TARGETS combine_grids 106 | EXPORT export_combine_grids 107 | ARCHIVE DESTINATION lib 108 | LIBRARY DESTINATION lib 109 | RUNTIME DESTINATION bin 110 | INCLUDES DESTINATION include 111 | ) 112 | 113 | install(TARGETS map_merge 114 | DESTINATION lib/${PROJECT_NAME}) 115 | 116 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 117 | 118 | 119 | ############# 120 | ## Testing ## 121 | ############# 122 | if(BUILD_TESTING) 123 | find_package(ament_lint_auto REQUIRED) 124 | # the following line skips the linter which checks for copyrights 125 | set(ament_cmake_copyright_FOUND TRUE) 126 | set(ament_cmake_cpplint_FOUND TRUE) 127 | ament_lint_auto_find_test_dependencies() 128 | 129 | find_package(ament_cmake_gtest REQUIRED) 130 | 131 | # download test data: TODO: ROS2 alternative? For now you'll need to download them manually 132 | set(base_url https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge) 133 | # ament_download(${base_url}/hector_maps/map00.pgm 134 | # MD5 915609a85793ec1375f310d44f2daf87 135 | # FILENAME ${PROJECT_NAME}_map00.pgm 136 | # ) 137 | execute_process( 138 | COMMAND bash download.sh ${CMAKE_BINARY_DIR}/map00.pgm ${base_url}/hector_maps/map00.pgm 915609a85793ec1375f310d44f2daf87 139 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test 140 | ) 141 | execute_process( 142 | COMMAND bash download.sh ${CMAKE_BINARY_DIR}/map05.pgm ${base_url}/hector_maps/map05.pgm cb9154c9fa3d97e5e992592daca9853a 143 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test 144 | ) 145 | execute_process( 146 | COMMAND bash download.sh ${CMAKE_BINARY_DIR}/2011-08-09-12-22-52.pgm ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm 3c2c38e7dec2b7a67f41069ab58badaa 147 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test 148 | ) 149 | execute_process( 150 | COMMAND bash download.sh ${CMAKE_BINARY_DIR}/2012-01-28-11-12-01.pgm ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm 681e704044889c95e47b0c3aadd81f1e 151 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test 152 | ) 153 | 154 | ament_add_gtest(test_merging_pipeline test/test_merging_pipeline.cpp) 155 | # ensure that test data are downloaded before we run tests 156 | # add_dependencies(test_merging_pipeline ${PROJECT_NAME}_map00.pgm ${PROJECT_NAME}_map05.pgm ${PROJECT_NAME}_2011-08-09-12-22-52.pgm ${PROJECT_NAME}_2012-01-28-11-12-01.pgm) 157 | target_link_libraries(test_merging_pipeline combine_grids ${catkin_LIBRARIES}) 158 | 159 | endif() 160 | 161 | 162 | ament_export_include_directories(include) 163 | ament_export_libraries(combine_grids) 164 | ament_package() -------------------------------------------------------------------------------- /map_merge/config/params.yaml: -------------------------------------------------------------------------------- 1 | map_merge: 2 | ros__parameters: 3 | merging_rate: 4.0 4 | discovery_rate: 0.05 5 | estimation_rate: 0.5 6 | estimation_confidence: 0.6 7 | robot_map_topic: map 8 | robot_map_updates_topic: map_updates 9 | robot_namespace: "" 10 | merged_map_topic: map 11 | world_frame: world 12 | known_init_poses: true 13 | # known_init_poses: false 14 | 15 | # Define here robots' positions in the map if known_init_poses is true 16 | /robot1/map_merge/init_pose_x: 0.0 17 | /robot1/map_merge/init_pose_y: 0.0 18 | /robot1/map_merge/init_pose_z: 0.0 19 | /robot1/map_merge/init_pose_yaw: 0.0 20 | 21 | /robot2/map_merge/init_pose_x: -3.0 22 | /robot2/map_merge/init_pose_y: 1.0 23 | /robot2/map_merge/init_pose_z: 0.0 24 | /robot2/map_merge/init_pose_yaw: 0.0 25 | -------------------------------------------------------------------------------- /map_merge/doc/architecture.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robo-friends/m-explore-ros2/9c121aebe52d28f3846a1cbb6ebb017cc85f9e53/map_merge/doc/architecture.dia -------------------------------------------------------------------------------- /map_merge/doc/screenshot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robo-friends/m-explore-ros2/9c121aebe52d28f3846a1cbb6ebb017cc85f9e53/map_merge/doc/screenshot.jpg -------------------------------------------------------------------------------- /map_merge/doc/wiki_doc.txt: -------------------------------------------------------------------------------- 1 | <> 2 | 3 | <> 4 | 5 | <> 6 | 7 | == Overview == 8 | This package provides global map for multiple robots. It can merge maps from arbitrary number of robots. It expects maps from individual robots as ROS topics. If your run multiple robots under the same ROS master then {{{multirobot_map_merge}}} will probably work for you out-of-the-box. It is also very easy to setup an simulation experiment. 9 | 10 | {{attachment:screenshot.jpg||width="755px"}} 11 | 12 | If your run your robots under multiple ROS masters you need to run your own way of communication between robots and provide maps from robots on local topics (under the same master). Also if you want to distribute merged map back to robots your communication must take care of it. 13 | 14 | <> 15 | 16 | {{{multirobot_map_merge}}} does not depend on any particular communication between robots. 17 | 18 | == Architecture == 19 | 20 | {{{multirobot_map_merge}}} finds robot maps dynamically and new robots can be added to system at any time. 21 | 22 | {{attachment:architecture.svg||width="755px"}} 23 | 24 | To make this dynamic behaviour possible there are some constrains placed on robots. First all robots must publish map under `/map`, where topic name (`map`) is configurable, but must be same for all robots. For each robot `` will be of cause different. 25 | 26 | This node support merging maps with known initial positions of the robots or without. See below for details. 27 | 28 | == Merging modes == 29 | 30 | Two merging modes are currently supported as orthogonal options. If you know initial positions of robots you may preferably use the first mode and get exact results (rigid transformation will be computed according to initial positions). If you don't know robot's starting points you are still able to use the second mode where transformation between grids will be determined using heuristic algorithm. You can choose between these two modes using the `known_init_poses` parameter. 31 | 32 | === merging with known initial positions === 33 | 34 | This is preferred mode whenever you are able to determine exact starting point for each robot. You need to provide initial position for each robot. You need to provide set of `/map_merge/init_pose` parameters. These positions should be in `world_frame`. See [[#ROS API]]. 35 | 36 | In this merging these parameters are mandatory. If any of the required parameters is missing robot won't be considered for merging (you will get warning with name of affected robot). 37 | 38 | === merging without known initial positions === 39 | 40 | If you can't provide initial poses for robots this mode has minimal configuration requirements. You need to provide only map topic for each robot. Transformation between grids is estimated by feature-matching algorithm and therefore requires grids to have sufficient amount of overlapping space to make a high-probability match. If grids don't have enough overlapping space to make a solid match, merged map can differ greatly from physical situation. 41 | 42 | Estimating transforms between grids is cpu-intesive so you might want to tune `estimation_rate` parameter to run re-estimation less often if it causes any troubles. 43 | 44 | == ROS API == 45 | {{{ 46 | #!clearsilver CS/NodeAPI 47 | 48 | name = map_merge 49 | desc = Provides map merging services offered by this package. Dynamically looks for new robots in the system and merges their maps. 50 | 51 | pub { 52 | 0.name = map 53 | 0.type = nav_msgs/OccupancyGrid 54 | 0.desc = Merged map from all robots in the system. 55 | } 56 | sub { 57 | 0.name = /map 58 | 0.type = nav_msgs/OccupancyGrid 59 | 0.desc = Local map for specific robot. 60 | 61 | 1.name = /map_updates 62 | 1.type = map_msgs/OccupancyGridUpdate 63 | 1.desc = Local map updates for specific robot. Most of the <> sources (mapping algorithms) provides incremental map updates via this topic so they don't need to send always full map. This topic is optional. If your mapping algorithm does not provide this topic it is safe to ignore this topic. However if your mapping algorithm does provide this topic, it is preferable to subscribe to this topic. Otherwise map updates will be slow as all partial updates will be missed and map will be able to update only on full map updates. 64 | } 65 | 66 | param { 67 | group.0 { 68 | name = ROBOT PARAMETERS 69 | desc = Parameters that should be defined in the namespace of each robot if you want to use merging with known initial poses of robots (`known_init_poses` is `true`). Without these parameters robots won't be considered for merging. If you can't provide these parameters use merging without known initial poses. See [[#Merging modes]] 70 | 71 | 0.name = /map_merge/init_pose_x 72 | 0.default = `` 73 | 0.type = double 74 | 0.desc = `x` coordinate of robot initial position in `world_frame`. Should be in meters. It does not matter which frame you will consider global (preferably it should be different from all robots frames), but relative positions of robots in this frame must be correct. 75 | 76 | 1.name = /map_merge/init_pose_y 77 | 1.default = `` 78 | 1.type = double 79 | 1.desc = `y` coordinate of robot initial position in `world_frame`. 80 | 81 | 2.name = /map_merge/init_pose_z 82 | 2.default = `` 83 | 2.type = double 84 | 2.desc = `z` coordinate of robot initial position in `world_frame`. 85 | 86 | 3.name = /map_merge/init_pose_yaw 87 | 3.default = `` 88 | 3.type = double 89 | 3.desc = `yaw` component of robot initial position in `world_frame`. Represents robot rotation in radians. 90 | } 91 | 92 | group.1 { 93 | name = NODE PARAMETERS 94 | desc = Parameters that should be defined in the namespace of this node. 95 | 96 | 0.name = ~robot_map_topic 97 | 0.default = `map` 98 | 0.type = string 99 | 0.desc = Name of robot map topic without namespaces (last component of topic name). Only topics with this name will be considered when looking for new maps to merge. This topics may be subject to further filtering (see below). 100 | 101 | 1.name = ~robot_map_updates_topic 102 | 1.default = `map_updates` 103 | 1.type = string 104 | 1.desc = Name of robot map updates topic of <> without namespaces (last component of topic name). This topic will be always subscribed in the same namespace as `robot_map_topic`. You'll likely need to change this only when you changed `robot_map_topic`. These topics are never considered when searching for new robots. 105 | 106 | 2.name = ~robot_namespace 107 | 2.default = `` 108 | 2.type = string 109 | 2.desc = Fixed part of robot map topic. You can employ this parameter to further limit which topics will be considered during dynamic lookup for robots. Only topics which contain (anywhere) this string will be considered for lookup. Unlike `robot_map_topic` you are not limited by namespace logic. Topics will be filtered using text-based search. Therefore `robot_namespace` does not need to be ROS namespace, but can contain slashes etc. This must be common part of all robots map topics name (all robots for which you want to merge map). 110 | 111 | 3.name = ~known_init_poses 112 | 3.default = `true` 113 | 3.type = bool 114 | 3.desc = Selects between merging modes. `true` if merging with known initial positions. See [[#Merging modes]] 115 | 116 | 4.name = ~merged_map_topic 117 | 4.default = `map` 118 | 4.type = string 119 | 4.desc = Topic name where merged map will be published. 120 | 121 | 5.name = ~world_frame 122 | 5.default = `world` 123 | 5.type = string 124 | 5.desc = Frame id (in [[tf]] tree) which will be assigned to published merged map. This should be frame where you specified robot initial positions. 125 | 126 | 6.name = ~merging_rate 127 | 6.default = `4.0` 128 | 6.type = double 129 | 6.desc = Rate in Hz. Basic frequency on which this node discovers merges robots maps and publish merged map. Increase this value if you want faster updates. 130 | 131 | 7.name = ~discovery_rate 132 | 7.default = `0.05` 133 | 7.type = double 134 | 7.desc = Rate in Hz. Frequency on which this node discovers new robots. Increase this value if you need more agile behaviour when adding new robots. Robots will be discovered sooner. 135 | 136 | 8.name = ~estimation_rate 137 | 8.default = `0.5` 138 | 8.type = double 139 | 8.desc = Rate in Hz. This parameter is relevant only when merging without known positions, see [[#Merging modes]]. Frequency on which this node re-estimates transformation between grids. Estimation is cpu-intensive, so you may wish to lower this value. 140 | 141 | 9.name = ~estimation_confidence 142 | 9.default = `1.0` 143 | 9.type = double 144 | 9.desc = This parameter is relevant only when merging without known positions, see [[#Merging modes]]. Confidence according to probabilistic model for initial positions estimation. Default value 1.0 is suitable for most applications, increase this value for more confident estimations. Number of maps included in the merge may decrease with increasing confidence. Generally larger overlaps between maps will be required for map to be included in merge. Good range for tuning is [1.0, 2.0]. 145 | } 146 | } 147 | }}} 148 | 149 | == Acknowledgements == 150 | 151 | This package was developed as part of my bachelor thesis at [[http://www.mff.cuni.cz/to.en/|Charles University]] in Prague. 152 | 153 | {{{ 154 | @masterthesis{Hörner2016, 155 | author = {Jiří Hörner}, 156 | title = {Map-merging for multi-robot system}, 157 | address = {Prague}, 158 | year = {2016}, 159 | school = {Charles University in Prague, Faculty of Mathematics and Physics}, 160 | type = {Bachelor's thesis}, 161 | URL = {https://is.cuni.cz/webapps/zzp/detail/174125/}, 162 | } 163 | }}} 164 | 165 | Idea for dynamic robot discovery is from [[map_merging]] package from Zhi Yan. Merging algorithm and configuration are different. 166 | 167 | ## AUTOGENERATED DON'T DELETE 168 | ## CategoryPackage 169 | -------------------------------------------------------------------------------- /map_merge/include/combine_grids/grid_compositor.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * Copyright (c) 2021, Carlos Alvarez. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #ifndef GRID_COMPOSITOR_H_ 39 | #define GRID_COMPOSITOR_H_ 40 | 41 | #include 42 | 43 | #include 44 | 45 | namespace combine_grids 46 | { 47 | namespace internal 48 | { 49 | class GridCompositor 50 | { 51 | public: 52 | nav_msgs::msg::OccupancyGrid::SharedPtr compose(const std::vector& grids, 53 | const std::vector& rois); 54 | }; 55 | 56 | } // namespace internal 57 | 58 | } // namespace combine_grids 59 | 60 | #endif // GRID_COMPOSITOR_H_ 61 | -------------------------------------------------------------------------------- /map_merge/include/combine_grids/grid_warper.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * Copyright (c) 2021, Carlos Alvarez. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #ifndef GRID_WARPER_H_ 39 | #define GRID_WARPER_H_ 40 | 41 | #include 42 | 43 | namespace combine_grids 44 | { 45 | namespace internal 46 | { 47 | class GridWarper 48 | { 49 | public: 50 | cv::Rect warp(const cv::Mat& grid, const cv::Mat& transform, 51 | cv::Mat& warped_grid); 52 | 53 | private: 54 | cv::Rect warpRoi(const cv::Mat& grid, const cv::Mat& transform); 55 | }; 56 | 57 | } // namespace internal 58 | 59 | } // namespace combine_grids 60 | 61 | #endif // GRID_WARPER_H_ 62 | -------------------------------------------------------------------------------- /map_merge/include/combine_grids/merging_pipeline.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * Copyright (c) 2021, Carlos Alvarez. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #ifndef MERGING_PIPELINE_H_ 39 | #define MERGING_PIPELINE_H_ 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | 49 | namespace combine_grids 50 | { 51 | enum class FeatureType { AKAZE, ORB, SURF }; 52 | 53 | /** 54 | * @brief Pipeline for merging overlapping occupancy grids 55 | * @details Pipeline works on internally stored grids. 56 | */ 57 | class MergingPipeline 58 | { 59 | public: 60 | template 61 | void feed(InputIt grids_begin, InputIt grids_end); 62 | bool estimateTransforms(FeatureType feature = FeatureType::AKAZE, 63 | double confidence = 1.0); 64 | nav_msgs::msg::OccupancyGrid::SharedPtr composeGrids(); 65 | 66 | std::vector getTransforms() const; 67 | template 68 | bool setTransforms(InputIt transforms_begin, InputIt transforms_end); 69 | 70 | private: 71 | std::vector grids_; 72 | std::vector images_; 73 | std::vector transforms_; 74 | double max_confidence_achieved_ = 0.0; 75 | }; 76 | 77 | template 78 | void MergingPipeline::feed(InputIt grids_begin, InputIt grids_end) 79 | { 80 | static_assert(std::is_assignable::value, 82 | "grids_begin must point to nav_msgs::msg::OccupancyGrid::ConstSharedPtr " 83 | "data"); 84 | 85 | // we can't reserve anything, because we want to support just InputIt and 86 | // their guarantee validity for only single-pass algos 87 | images_.clear(); 88 | grids_.clear(); 89 | for (InputIt it = grids_begin; it != grids_end; ++it) { 90 | if (*it && !(*it)->data.empty()) { 91 | grids_.push_back(*it); 92 | /* convert to opencv images. it creates only a view for opencv and does 93 | * not copy or own actual data. */ 94 | images_.emplace_back((*it)->info.height, (*it)->info.width, CV_8UC1, 95 | const_cast((*it)->data.data())); 96 | } else { 97 | grids_.emplace_back(); 98 | images_.emplace_back(); 99 | } 100 | } 101 | } 102 | 103 | template 104 | bool MergingPipeline::setTransforms(InputIt transforms_begin, 105 | InputIt transforms_end) 106 | { 107 | static_assert(std::is_assignable::value, 109 | "transforms_begin must point to geometry_msgs::msg::Transform " 110 | "data"); 111 | 112 | decltype(transforms_) transforms_buf; 113 | for (InputIt it = transforms_begin; it != transforms_end; ++it) { 114 | const geometry_msgs::msg::Quaternion& q = it->rotation; 115 | if ((q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) < 116 | std::numeric_limits::epsilon()) { 117 | // represents invalid transform 118 | transforms_buf.emplace_back(); 119 | continue; 120 | } 121 | double s = 2.0 / (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); 122 | double a = 1 - q.y * q.y * s - q.z * q.z * s; 123 | double b = q.x * q.y * s + q.z * q.w * s; 124 | double tx = it->translation.x; 125 | double ty = it->translation.y; 126 | cv::Mat transform = cv::Mat::eye(3, 3, CV_64F); 127 | transform.at(0, 0) = transform.at(1, 1) = a; 128 | transform.at(1, 0) = b; 129 | transform.at(0, 1) = -b; 130 | transform.at(0, 2) = tx; 131 | transform.at(1, 2) = ty; 132 | 133 | transforms_buf.emplace_back(std::move(transform)); 134 | } 135 | 136 | if (transforms_buf.size() != images_.size()) { 137 | return false; 138 | } 139 | std::swap(transforms_, transforms_buf); 140 | 141 | return true; 142 | } 143 | 144 | } // namespace combine_grids 145 | 146 | #endif // MERGING_PIPELINE_H_ 147 | -------------------------------------------------------------------------------- /map_merge/include/map_merge/map_merge.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2014, Zhi Yan. 6 | * Copyright (c) 2015-2016, Jiri Horner. 7 | * Copyright (c) 2021, Carlos Alvarez. 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the Jiri Horner nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | *********************************************************************/ 38 | 39 | #ifndef MAP_MERGE_H_ 40 | #define MAP_MERGE_H_ 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | 55 | namespace map_merge 56 | { 57 | struct MapSubscription { 58 | // protects consistency of writable_map and readonly_map 59 | // also protects reads and writes of shared_ptrs 60 | std::mutex mutex; 61 | 62 | geometry_msgs::msg::Transform initial_pose; 63 | nav_msgs::msg::OccupancyGrid::SharedPtr writable_map; 64 | nav_msgs::msg::OccupancyGrid::ConstSharedPtr readonly_map; 65 | 66 | // ros::Subscriber map_sub; 67 | // ros::Subscriber map_updates_sub; 68 | rclcpp::Subscription::SharedPtr map_sub; 69 | rclcpp::Subscription::SharedPtr map_updates_sub; 70 | }; 71 | 72 | class MapMerge : public rclcpp::Node 73 | { 74 | private: 75 | /* parameters */ 76 | double merging_rate_; 77 | double discovery_rate_; 78 | double estimation_rate_; 79 | double confidence_threshold_; 80 | std::string robot_map_topic_; 81 | std::string robot_map_updates_topic_; 82 | std::string robot_namespace_; 83 | std::string world_frame_; 84 | bool have_initial_poses_; 85 | 86 | // publishing 87 | rclcpp::Publisher::SharedPtr merged_map_publisher_; 88 | // maps robots namespaces to maps. does not own 89 | std::unordered_map robots_; 90 | // owns maps -- iterator safe 91 | std::forward_list subscriptions_; 92 | size_t subscriptions_size_; 93 | boost::shared_mutex subscriptions_mutex_; 94 | combine_grids::MergingPipeline pipeline_; 95 | std::mutex pipeline_mutex_; 96 | 97 | rclcpp::Logger logger_ = rclcpp::get_logger("MapMergeNode"); 98 | 99 | // timers 100 | rclcpp::TimerBase::SharedPtr map_merging_timer_; 101 | rclcpp::TimerBase::SharedPtr topic_subscribing_timer_; 102 | rclcpp::TimerBase::SharedPtr pose_estimation_timer_; 103 | 104 | std::string robotNameFromTopic(const std::string& topic); 105 | // bool isRobotMapTopic(const ros::master::TopicInfo& topic); 106 | bool isRobotMapTopic(const std::string topic, std::string type); 107 | bool getInitPose(const std::string& name, geometry_msgs::msg::Transform& pose); 108 | 109 | void fullMapUpdate(const nav_msgs::msg::OccupancyGrid::SharedPtr msg, 110 | MapSubscription& map); 111 | void partialMapUpdate(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg, 112 | MapSubscription& map); 113 | 114 | public: 115 | MapMerge(); 116 | 117 | void topicSubscribing(); 118 | void mapMerging(); 119 | /** 120 | * @brief Estimates initial positions of grids 121 | * @details Relevant only if initial poses are not known 122 | */ 123 | void poseEstimation(); 124 | }; 125 | 126 | } // namespace map_merge 127 | 128 | #endif /* MAP_MERGE_H_ */ 129 | -------------------------------------------------------------------------------- /map_merge/include/map_merge/ros1_names.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | #include 28 | #include 29 | #include 30 | 31 | // Almost all code was taken from http://docs.ros.org/en/indigo/api/roscpp/html/names_8cpp_source.html 32 | 33 | class InvalidNameException : public std::runtime_error 34 | { 35 | public: 36 | InvalidNameException(const std::string& msg) 37 | : std::runtime_error(msg) 38 | {} 39 | }; 40 | 41 | namespace ros1_names 42 | { 43 | 44 | bool isValidCharInName(char c) 45 | { 46 | if (isalnum(c) || c == '/' || c == '_') 47 | { 48 | return true; 49 | } 50 | 51 | return false; 52 | } 53 | 54 | bool validate(const std::string& name, std::string& error) 55 | { 56 | if (name.empty()) 57 | { 58 | return true; 59 | } 60 | 61 | // First element is special, can be only ~ / or alpha 62 | char c = name[0]; 63 | if (!isalpha(c) && c != '/' && c != '~') 64 | { 65 | std::stringstream ss; 66 | ss << "Character [" << c << "] is not valid as the first character in Graph Resource Name [" << name << "]. Valid characters are a-z, A-Z, / and in some cases ~."; 67 | error = ss.str(); 68 | return false; 69 | } 70 | 71 | for (size_t i = 1; i < name.size(); ++i) 72 | { 73 | c = name[i]; 74 | if (!isValidCharInName(c)) 75 | { 76 | std::stringstream ss; 77 | ss << "Character [" << c << "] at element [" << i << "] is not valid in Graph Resource Name [" << name <<"]. Valid characters are a-z, A-Z, 0-9, / and _."; 78 | error = ss.str(); 79 | 80 | return false; 81 | } 82 | } 83 | 84 | return true; 85 | } 86 | 87 | std::string parentNamespace(const std::string& name) 88 | { 89 | std::string error; 90 | if (!validate(name, error)) 91 | { 92 | throw InvalidNameException(error); 93 | } 94 | 95 | if (!name.compare("")) return ""; 96 | if (!name.compare("/")) return "/"; 97 | 98 | std::string stripped_name; 99 | 100 | // rstrip trailing slash 101 | if (name.find_last_of('/') == name.size()-1) 102 | stripped_name = name.substr(0, name.size() -2); 103 | else 104 | stripped_name = name; 105 | 106 | //pull everything up to the last / 107 | size_t last_pos = stripped_name.find_last_of('/'); 108 | if (last_pos == std::string::npos) 109 | { 110 | return ""; 111 | } 112 | else if (last_pos == 0) 113 | return "/"; 114 | return stripped_name.substr(0, last_pos); 115 | } 116 | 117 | std::string clean(const std::string& name) 118 | { 119 | std::string clean = name; 120 | 121 | size_t pos = clean.find("//"); 122 | while (pos != std::string::npos) 123 | { 124 | clean.erase(pos, 1); 125 | pos = clean.find("//", pos); 126 | } 127 | 128 | if (*clean.rbegin() == '/') 129 | { 130 | clean.erase(clean.size() - 1, 1); 131 | } 132 | 133 | return clean; 134 | } 135 | 136 | std::string append(const std::string& left, const std::string& right) 137 | { 138 | return clean(left + "/" + right); 139 | } 140 | 141 | } // namespace ros1_names -------------------------------------------------------------------------------- /map_merge/launch/from_map_server.launch.py: -------------------------------------------------------------------------------- 1 | # showcases map_merge with static maps served by map_server 2 | 3 | # you can run this with test maps provided in m-explore-extra repo 4 | # https://github.com/hrnr/m-explore-extra 5 | 6 | # roslaunch multirobot_map_merge from_map_server.launch map1:=PATH_TO_m-explore-extra/map_merge/gmapping_maps/2011-08-09-12-22-52.yaml map2:=PATH_TO_m-explore-extra/map_merge/gmapping_maps/2012-01-28-11-12-01.yaml rviz:=True 7 | 8 | import os 9 | 10 | from ament_index_python.packages import get_package_share_directory 11 | from launch_ros.actions import Node, PushRosNamespace 12 | 13 | from launch import LaunchDescription 14 | from launch.actions import ( 15 | DeclareLaunchArgument, 16 | EmitEvent, 17 | GroupAction, 18 | RegisterEventHandler, 19 | ) 20 | from launch.conditions import IfCondition 21 | from launch.event_handlers import OnProcessExit 22 | from launch.events import Shutdown 23 | from launch.substitutions import LaunchConfiguration 24 | 25 | 26 | def generate_launch_description(): 27 | ld = LaunchDescription() 28 | config = os.path.join( 29 | get_package_share_directory("multirobot_map_merge"), "config", "params.yaml" 30 | ) 31 | use_sim_time = LaunchConfiguration("use_sim_time") 32 | namespace = LaunchConfiguration("namespace") 33 | known_init_poses = LaunchConfiguration("known_init_poses") 34 | rviz = LaunchConfiguration("rviz") 35 | 36 | declare_use_sim_time_argument = DeclareLaunchArgument( 37 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 38 | ) 39 | declare_namespace_argument = DeclareLaunchArgument( 40 | "namespace", 41 | default_value="/", 42 | description="Namespace for the explore node", 43 | ) 44 | declare_known_init_poses_argument = DeclareLaunchArgument( 45 | "known_init_poses", 46 | default_value="False", 47 | description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file", 48 | ) 49 | declare_rviz_argument = DeclareLaunchArgument( 50 | "rviz", 51 | default_value="False", 52 | description="Run rviz2", 53 | ) 54 | 55 | num_maps = 2 56 | group_actions = [] 57 | 58 | for i in range(num_maps): 59 | map_argument_name = f"map{i+1}" 60 | map_yaml_file = LaunchConfiguration(map_argument_name) 61 | declare_map_argument = DeclareLaunchArgument( 62 | map_argument_name, 63 | default_value=f"{map_argument_name}.yaml", 64 | description="Full path to map yaml file to load", 65 | ) 66 | map_server = Node( 67 | package="nav2_map_server", 68 | executable="map_server", 69 | name="map_server", 70 | namespace=f"robot{i+1}", 71 | output="screen", 72 | parameters=[ 73 | {"yaml_filename": map_yaml_file}, 74 | {"use_sim_time": use_sim_time}, 75 | ], 76 | ) 77 | map_server_manager = Node( 78 | package="nav2_lifecycle_manager", 79 | executable="lifecycle_manager", 80 | name="lifecycle_manager_localization", 81 | output="screen", 82 | namespace=f"robot{i+1}", 83 | parameters=[ 84 | {"use_sim_time": use_sim_time}, 85 | {"autostart": True}, 86 | {"node_names": ["map_server"]}, 87 | ], 88 | ) 89 | group_action = GroupAction( 90 | [ 91 | PushRosNamespace(namespace=namespace), 92 | map_server_manager, 93 | map_server, 94 | declare_map_argument, 95 | ] 96 | ) 97 | 98 | group_actions.append(group_action) 99 | 100 | node = Node( 101 | package="multirobot_map_merge", 102 | name="map_merge", 103 | namespace=namespace, 104 | executable="map_merge", 105 | parameters=[ 106 | config, 107 | {"use_sim_time": use_sim_time}, 108 | {"known_init_poses": known_init_poses}, 109 | ], 110 | output="screen", 111 | ) 112 | 113 | rviz_config_file = os.path.join( 114 | get_package_share_directory("multirobot_map_merge"), "launch", "map_merge.rviz" 115 | ) 116 | start_rviz_cmd = Node( 117 | condition=IfCondition(rviz), 118 | package="rviz2", 119 | executable="rviz2", 120 | arguments=["-d", rviz_config_file], 121 | output="screen", 122 | ) 123 | exit_event_handler = RegisterEventHandler( 124 | condition=IfCondition(rviz), 125 | event_handler=OnProcessExit( 126 | target_action=start_rviz_cmd, 127 | on_exit=EmitEvent(event=Shutdown(reason="rviz exited")), 128 | ), 129 | ) 130 | 131 | ld.add_action(declare_use_sim_time_argument) 132 | ld.add_action(declare_known_init_poses_argument) 133 | ld.add_action(declare_namespace_argument) 134 | ld.add_action(declare_rviz_argument) 135 | for group_action in group_actions: 136 | ld.add_action(group_action) 137 | ld.add_action(node) 138 | ld.add_action(start_rviz_cmd) 139 | ld.add_action(exit_event_handler) 140 | 141 | return ld 142 | -------------------------------------------------------------------------------- /map_merge/launch/map_merge.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch_ros.actions import Node 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import DeclareLaunchArgument 8 | from launch.substitutions import LaunchConfiguration 9 | 10 | 11 | def generate_launch_description(): 12 | ld = LaunchDescription() 13 | config = os.path.join( 14 | get_package_share_directory("multirobot_map_merge"), "config", "params.yaml" 15 | ) 16 | use_sim_time = LaunchConfiguration("use_sim_time") 17 | namespace = LaunchConfiguration("namespace") 18 | known_init_poses = LaunchConfiguration("known_init_poses") 19 | 20 | declare_use_sim_time_argument = DeclareLaunchArgument( 21 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 22 | ) 23 | declare_namespace_argument = DeclareLaunchArgument( 24 | "namespace", 25 | default_value="/", 26 | description="Namespace for the explore node", 27 | ) 28 | declare_known_init_poses_argument = DeclareLaunchArgument( 29 | "known_init_poses", 30 | default_value="True", 31 | description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file", 32 | ) 33 | 34 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 35 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 36 | # https://github.com/ros/geometry2/issues/32 37 | # https://github.com/ros/robot_state_publisher/pull/30 38 | remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] 39 | 40 | node = Node( 41 | package="multirobot_map_merge", 42 | name="map_merge", 43 | namespace=namespace, 44 | executable="map_merge", 45 | parameters=[ 46 | config, 47 | {"use_sim_time": use_sim_time}, 48 | {"known_init_poses": known_init_poses}, 49 | ], 50 | output="screen", 51 | remappings=remappings, 52 | ) 53 | ld.add_action(declare_use_sim_time_argument) 54 | ld.add_action(declare_known_init_poses_argument) 55 | ld.add_action(declare_namespace_argument) 56 | ld.add_action(node) 57 | return ld 58 | -------------------------------------------------------------------------------- /map_merge/launch/map_merge.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 1123 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Alpha: 0.699999988079071 51 | Class: rviz_default_plugins/Map 52 | Color Scheme: map 53 | Draw Behind: false 54 | Enabled: true 55 | Name: Map 56 | Topic: 57 | Depth: 5 58 | Durability Policy: Volatile 59 | Filter size: 10 60 | History Policy: Keep Last 61 | Reliability Policy: Reliable 62 | Value: /map 63 | Update Topic: 64 | Depth: 5 65 | Durability Policy: Volatile 66 | History Policy: Keep Last 67 | Reliability Policy: Reliable 68 | Value: /map_updates 69 | Use Timestamp: false 70 | Value: true 71 | Enabled: true 72 | Global Options: 73 | Background Color: 48; 48; 48 74 | Fixed Frame: world 75 | Frame Rate: 30 76 | Name: root 77 | Tools: 78 | - Class: rviz_default_plugins/Interact 79 | Hide Inactive Objects: true 80 | - Class: rviz_default_plugins/MoveCamera 81 | - Class: rviz_default_plugins/Select 82 | - Class: rviz_default_plugins/FocusCamera 83 | - Class: rviz_default_plugins/Measure 84 | Line color: 128; 128; 0 85 | - Class: rviz_default_plugins/SetInitialPose 86 | Covariance x: 0.25 87 | Covariance y: 0.25 88 | Covariance yaw: 0.06853891909122467 89 | Topic: 90 | Depth: 5 91 | Durability Policy: Volatile 92 | History Policy: Keep Last 93 | Reliability Policy: Reliable 94 | Value: /initialpose 95 | - Class: rviz_default_plugins/SetGoal 96 | Topic: 97 | Depth: 5 98 | Durability Policy: Volatile 99 | History Policy: Keep Last 100 | Reliability Policy: Reliable 101 | Value: /goal_pose 102 | - Class: rviz_default_plugins/PublishPoint 103 | Single click: true 104 | Topic: 105 | Depth: 5 106 | Durability Policy: Volatile 107 | History Policy: Keep Last 108 | Reliability Policy: Reliable 109 | Value: /clicked_point 110 | Transformation: 111 | Current: 112 | Class: rviz_default_plugins/TF 113 | Value: true 114 | Views: 115 | Current: 116 | Class: rviz_default_plugins/Orbit 117 | Distance: 33.400630950927734 118 | Enable Stereo Rendering: 119 | Stereo Eye Separation: 0.05999999865889549 120 | Stereo Focal Distance: 1 121 | Swap Stereo Eyes: false 122 | Value: false 123 | Focal Point: 124 | X: 0.5996218919754028 125 | Y: 0.5541106462478638 126 | Z: -0.0447520911693573 127 | Focal Shape Fixed Size: true 128 | Focal Shape Size: 0.05000000074505806 129 | Invert Z Axis: false 130 | Name: Current View 131 | Near Clip Distance: 0.009999999776482582 132 | Pitch: 1.5697963237762451 133 | Target Frame: 134 | Value: Orbit (rviz) 135 | Yaw: 3.1304056644439697 136 | Saved: ~ 137 | Window Geometry: 138 | Displays: 139 | collapsed: false 140 | Height: 1600 141 | Hide Left Dock: false 142 | Hide Right Dock: true 143 | QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000551fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000005510000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f000003aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000069000003ae0000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ae00000051fc0100000002fb0000000800540069006d00650100000000000004ae0000045300fffffffb0000000800540069006d00650100000000000004500000000000000000000002c60000055100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 144 | Selection: 145 | collapsed: false 146 | Time: 147 | collapsed: false 148 | Tool Properties: 149 | collapsed: false 150 | Views: 151 | collapsed: true 152 | Width: 1198 153 | X: 1322 154 | Y: 115 155 | -------------------------------------------------------------------------------- /map_merge/launch/tb3_simulation/bringup_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import ( 21 | DeclareLaunchArgument, 22 | GroupAction, 23 | IncludeLaunchDescription, 24 | SetEnvironmentVariable, 25 | ) 26 | from launch.conditions import IfCondition 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch.substitutions import LaunchConfiguration, PythonExpression 29 | from launch_ros.actions import PushRosNamespace 30 | 31 | 32 | def generate_launch_description(): 33 | # Get the launch directory 34 | bringup_dir = get_package_share_directory("nav2_bringup") 35 | launch_dir = os.path.join(bringup_dir, "launch") 36 | 37 | # Get the launch directory of gmapping 38 | slam_gmapping_dir = get_package_share_directory("slam_gmapping") 39 | slam_gmapping_launch_dir = os.path.join(slam_gmapping_dir, "launch") 40 | 41 | # Get the launch directory of map_merge 42 | map_merge_dir = get_package_share_directory("multirobot_map_merge") 43 | map_merge_launch_dir = os.path.join(map_merge_dir, "launch", "tb3_simulation") 44 | 45 | # Create the launch configuration variables 46 | namespace = LaunchConfiguration("namespace") 47 | use_namespace = LaunchConfiguration("use_namespace") 48 | slam = LaunchConfiguration("slam") 49 | map_yaml_file = LaunchConfiguration("map") 50 | use_sim_time = LaunchConfiguration("use_sim_time") 51 | params_file = LaunchConfiguration("params_file") 52 | autostart = LaunchConfiguration("autostart") 53 | slam_toolbox = LaunchConfiguration("slam_toolbox") 54 | slam_gmapping = LaunchConfiguration("slam_gmapping") 55 | 56 | stdout_linebuf_envvar = SetEnvironmentVariable( 57 | "RCUTILS_LOGGING_BUFFERED_STREAM", "1" 58 | ) 59 | 60 | declare_namespace_cmd = DeclareLaunchArgument( 61 | "namespace", default_value="", description="Top-level namespace" 62 | ) 63 | 64 | declare_use_namespace_cmd = DeclareLaunchArgument( 65 | "use_namespace", 66 | default_value="false", 67 | description="Whether to apply a namespace to the navigation stack", 68 | ) 69 | 70 | declare_slam_cmd = DeclareLaunchArgument( 71 | "slam", default_value="False", description="Whether run a SLAM" 72 | ) 73 | declare_slam_toolbox_cmd = DeclareLaunchArgument( 74 | "slam_toolbox", default_value="False", description="Whether run a SLAM toolbox" 75 | ) 76 | declare_slam_gmapping_cmd = DeclareLaunchArgument( 77 | "slam_gmapping", 78 | default_value="False", 79 | description="Whether run a SLAM gmapping", 80 | ) 81 | 82 | declare_map_yaml_cmd = DeclareLaunchArgument( 83 | "map", description="Full path to map yaml file to load" 84 | ) 85 | 86 | declare_use_sim_time_cmd = DeclareLaunchArgument( 87 | "use_sim_time", 88 | default_value="false", 89 | description="Use simulation (Gazebo) clock if true", 90 | ) 91 | 92 | declare_params_file_cmd = DeclareLaunchArgument( 93 | "params_file", 94 | default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"), 95 | description="Full path to the ROS2 parameters file to use for all launched nodes", 96 | ) 97 | 98 | declare_autostart_cmd = DeclareLaunchArgument( 99 | "autostart", 100 | default_value="true", 101 | description="Automatically startup the nav2 stack", 102 | ) 103 | 104 | # Specify the actions 105 | bringup_cmd_group = GroupAction( 106 | [ 107 | PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), 108 | # IncludeLaunchDescription( 109 | # PythonLaunchDescriptionSource( 110 | # os.path.join(launch_dir, "slam_launch.py") 111 | # ), 112 | # condition=IfCondition( 113 | # PythonExpression( 114 | # [slam, " and ", slam_toolbox, " and not ", slam_gmapping] 115 | # ) 116 | # ), 117 | # launch_arguments={ 118 | # "namespace": namespace, 119 | # "use_sim_time": use_sim_time, 120 | # "autostart": autostart, 121 | # "params_file": params_file, 122 | # }.items(), 123 | # ), 124 | IncludeLaunchDescription( 125 | PythonLaunchDescriptionSource( 126 | os.path.join(map_merge_launch_dir, "slam_toolbox.py") 127 | ), 128 | condition=IfCondition( 129 | PythonExpression( 130 | [slam, " and ", slam_toolbox, " and not ", slam_gmapping] 131 | ) 132 | ), 133 | launch_arguments={ 134 | "use_sim_time": use_sim_time, 135 | }.items(), 136 | ), 137 | IncludeLaunchDescription( 138 | PythonLaunchDescriptionSource( 139 | os.path.join(launch_dir, "localization_launch.py") 140 | ), 141 | condition=IfCondition(PythonExpression(["not ", slam])), 142 | launch_arguments={ 143 | "namespace": namespace, 144 | "map": map_yaml_file, 145 | "use_sim_time": use_sim_time, 146 | "autostart": autostart, 147 | "params_file": params_file, 148 | "use_lifecycle_mgr": "false", 149 | }.items(), 150 | ), 151 | IncludeLaunchDescription( 152 | PythonLaunchDescriptionSource( 153 | os.path.join(launch_dir, "navigation_launch.py") 154 | ), 155 | launch_arguments={ 156 | "namespace": namespace, 157 | "use_sim_time": use_sim_time, 158 | "autostart": autostart, 159 | "params_file": params_file, 160 | "use_lifecycle_mgr": "false", 161 | "map_subscribe_transient_local": "true", 162 | }.items(), 163 | ), 164 | ] 165 | ) 166 | # Not in GroupAction because namespace were prepended twice because 167 | # slam_gmapping.launch.py already accepts a namespace argument 168 | slam_gmapping_cmd = IncludeLaunchDescription( 169 | PythonLaunchDescriptionSource( 170 | os.path.join(slam_gmapping_launch_dir, "slam_gmapping.launch.py") 171 | ), 172 | condition=IfCondition( 173 | PythonExpression([slam, " and ", slam_gmapping, " and not ", slam_toolbox]) 174 | ), 175 | launch_arguments={ 176 | "namespace": namespace, 177 | "use_sim_time": use_sim_time, 178 | }.items(), 179 | ) 180 | 181 | # Create the launch description and populate 182 | ld = LaunchDescription() 183 | 184 | # Set environment variables 185 | ld.add_action(stdout_linebuf_envvar) 186 | 187 | # Declare the launch options 188 | ld.add_action(declare_namespace_cmd) 189 | ld.add_action(declare_use_namespace_cmd) 190 | ld.add_action(declare_slam_cmd) 191 | ld.add_action(declare_slam_toolbox_cmd) 192 | ld.add_action(declare_slam_gmapping_cmd) 193 | ld.add_action(declare_map_yaml_cmd) 194 | ld.add_action(declare_use_sim_time_cmd) 195 | ld.add_action(declare_params_file_cmd) 196 | ld.add_action(declare_autostart_cmd) 197 | 198 | # Add the actions to launch all of the navigation nodes 199 | ld.add_action(bringup_cmd_group) 200 | ld.add_action(slam_gmapping_cmd) 201 | 202 | return ld 203 | -------------------------------------------------------------------------------- /map_merge/launch/tb3_simulation/config/nav2_multirobot_params_1.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "differential" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | 42 | amcl_map_client: 43 | ros__parameters: 44 | use_sim_time: True 45 | 46 | amcl_rclcpp_node: 47 | ros__parameters: 48 | use_sim_time: True 49 | 50 | bt_navigator: 51 | ros__parameters: 52 | use_sim_time: True 53 | global_frame: map 54 | robot_base_frame: base_link 55 | odom_topic: /odom 56 | bt_loop_duration: 10 57 | default_server_timeout: 20 58 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 59 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 60 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 61 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 62 | # if enable_groot_monitoring is set to True, ports need to be different for each robot !! 63 | enable_groot_monitoring: False 64 | groot_zmq_publisher_port: 1666 65 | groot_zmq_server_port: 1667 66 | plugin_lib_names: 67 | - nav2_compute_path_to_pose_action_bt_node 68 | - nav2_compute_path_through_poses_action_bt_node 69 | - nav2_follow_path_action_bt_node 70 | - nav2_back_up_action_bt_node 71 | - nav2_spin_action_bt_node 72 | - nav2_wait_action_bt_node 73 | - nav2_clear_costmap_service_bt_node 74 | - nav2_is_stuck_condition_bt_node 75 | - nav2_goal_reached_condition_bt_node 76 | - nav2_goal_updated_condition_bt_node 77 | - nav2_initial_pose_received_condition_bt_node 78 | - nav2_reinitialize_global_localization_service_bt_node 79 | - nav2_rate_controller_bt_node 80 | - nav2_distance_controller_bt_node 81 | - nav2_speed_controller_bt_node 82 | - nav2_truncate_path_action_bt_node 83 | - nav2_goal_updater_node_bt_node 84 | - nav2_recovery_node_bt_node 85 | - nav2_pipeline_sequence_bt_node 86 | - nav2_round_robin_node_bt_node 87 | - nav2_transform_available_condition_bt_node 88 | - nav2_time_expired_condition_bt_node 89 | - nav2_distance_traveled_condition_bt_node 90 | - nav2_single_trigger_bt_node 91 | - nav2_is_battery_low_condition_bt_node 92 | - nav2_navigate_through_poses_action_bt_node 93 | - nav2_navigate_to_pose_action_bt_node 94 | - nav2_remove_passed_goals_action_bt_node 95 | - nav2_planner_selector_bt_node 96 | - nav2_controller_selector_bt_node 97 | - nav2_goal_checker_selector_bt_node 98 | 99 | bt_navigator_rclcpp_node: 100 | ros__parameters: 101 | use_sim_time: True 102 | 103 | controller_server: 104 | ros__parameters: 105 | use_sim_time: True 106 | controller_frequency: 20.0 107 | min_x_velocity_threshold: 0.001 108 | min_y_velocity_threshold: 0.5 109 | min_theta_velocity_threshold: 0.001 110 | progress_checker_plugin: "progress_checker" 111 | goal_checker_plugin: "goal_checker" 112 | controller_plugins: ["FollowPath"] 113 | 114 | # Progress checker parameters 115 | progress_checker: 116 | plugin: "nav2_controller::SimpleProgressChecker" 117 | required_movement_radius: 0.5 118 | movement_time_allowance: 10.0 119 | # Goal checker parameters 120 | goal_checker: 121 | plugin: "nav2_controller::SimpleGoalChecker" 122 | xy_goal_tolerance: 0.25 123 | yaw_goal_tolerance: 0.25 124 | stateful: True 125 | # DWB parameters 126 | FollowPath: 127 | plugin: "dwb_core::DWBLocalPlanner" 128 | debug_trajectory_details: True 129 | min_vel_x: 0.0 130 | min_vel_y: 0.0 131 | max_vel_x: 0.26 132 | max_vel_y: 0.0 133 | max_vel_theta: 1.0 134 | min_speed_xy: 0.0 135 | max_speed_xy: 0.26 136 | min_speed_theta: 0.0 137 | # Add high threshold velocity for turtlebot 3 issue. 138 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 139 | acc_lim_x: 2.5 140 | acc_lim_y: 0.0 141 | acc_lim_theta: 3.2 142 | decel_lim_x: -2.5 143 | decel_lim_y: 0.0 144 | decel_lim_theta: -3.2 145 | vx_samples: 20 146 | vy_samples: 5 147 | vtheta_samples: 20 148 | sim_time: 1.7 149 | linear_granularity: 0.05 150 | angular_granularity: 0.025 151 | transform_tolerance: 0.2 152 | trans_stopped_velocity: 0.25 153 | short_circuit_trajectory_evaluation: True 154 | stateful: True 155 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 156 | BaseObstacle.scale: 0.02 157 | PathAlign.scale: 0.0 158 | GoalAlign.scale: 0.0 159 | PathDist.scale: 32.0 160 | GoalDist.scale: 24.0 161 | RotateToGoal.scale: 32.0 162 | RotateToGoal.slowing_factor: 5.0 163 | RotateToGoal.lookahead_time: -1.0 164 | 165 | controller_server_rclcpp_node: 166 | ros__parameters: 167 | use_sim_time: True 168 | 169 | local_costmap: 170 | local_costmap: 171 | ros__parameters: 172 | use_sim_time: True 173 | global_frame: odom 174 | rolling_window: true 175 | width: 3 176 | height: 3 177 | resolution: 0.05 178 | robot_radius: 0.22 179 | plugins: ["obstacle_layer", "inflation_layer"] 180 | inflation_layer: 181 | plugin: "nav2_costmap_2d::InflationLayer" 182 | cost_scaling_factor: 3.0 183 | inflation_radius: 0.55 184 | obstacle_layer: 185 | plugin: "nav2_costmap_2d::ObstacleLayer" 186 | enabled: True 187 | scan: 188 | topic: /robot1/scan 189 | max_obstacle_height: 2.0 190 | clearing: True 191 | marking: True 192 | raytrace_max_range: 3.0 193 | raytrace_min_range: 0.0 194 | obstacle_max_range: 2.5 195 | obstacle_min_range: 0.0 196 | static_layer: 197 | map_subscribe_transient_local: True 198 | always_send_full_costmap: True 199 | observation_sources: scan 200 | local_costmap_client: 201 | ros__parameters: 202 | use_sim_time: True 203 | local_costmap_rclcpp_node: 204 | ros__parameters: 205 | use_sim_time: True 206 | 207 | global_costmap: 208 | global_costmap: 209 | ros__parameters: 210 | use_sim_time: True 211 | robot_radius: 0.22 212 | obstacle_layer: 213 | enabled: True 214 | scan: 215 | topic: /robot1/scan 216 | max_obstacle_height: 2.0 217 | clearing: True 218 | marking: True 219 | raytrace_max_range: 3.0 220 | raytrace_min_range: 0.0 221 | obstacle_max_range: 2.5 222 | obstacle_min_range: 0.0 223 | static_layer: 224 | map_subscribe_transient_local: True 225 | always_send_full_costmap: True 226 | observation_sources: scan 227 | global_costmap_client: 228 | ros__parameters: 229 | use_sim_time: True 230 | global_costmap_rclcpp_node: 231 | ros__parameters: 232 | use_sim_time: True 233 | 234 | map_server: 235 | ros__parameters: 236 | use_sim_time: True 237 | yaml_filename: "turtlebot3_world.yaml" 238 | save_map_timeout: 5.0 239 | 240 | planner_server: 241 | ros__parameters: 242 | use_sim_time: True 243 | planner_plugins: ["GridBased"] 244 | GridBased: 245 | plugin: "nav2_navfn_planner/NavfnPlanner" 246 | tolerance: 0.5 247 | use_astar: false 248 | allow_unknown: true 249 | 250 | planner_server_rclcpp_node: 251 | ros__parameters: 252 | use_sim_time: True 253 | 254 | recoveries_server: 255 | ros__parameters: 256 | costmap_topic: local_costmap/costmap_raw 257 | footprint_topic: local_costmap/published_footprint 258 | cycle_frequency: 10.0 259 | recovery_plugins: ["spin", "backup", "wait"] 260 | spin: 261 | plugin: "nav2_recoveries/Spin" 262 | backup: 263 | plugin: "nav2_recoveries/BackUp" 264 | wait: 265 | plugin: "nav2_recoveries/Wait" 266 | global_frame: odom 267 | robot_base_frame: base_link 268 | transform_timeout: 0.1 269 | use_sim_time: true 270 | simulate_ahead_time: 2.0 271 | max_rotational_vel: 1.0 272 | min_rotational_vel: 0.4 273 | rotational_acc_lim: 3.2 274 | 275 | robot_state_publisher: 276 | ros__parameters: 277 | use_sim_time: True 278 | 279 | waypoint_follower: 280 | ros__parameters: 281 | loop_rate: 20 282 | stop_on_failure: false 283 | waypoint_task_executor_plugin: "wait_at_waypoint" 284 | wait_at_waypoint: 285 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 286 | enabled: True 287 | waypoint_pause_duration: 200 -------------------------------------------------------------------------------- /map_merge/launch/tb3_simulation/config/nav2_multirobot_params_2.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "differential" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | 42 | amcl_map_client: 43 | ros__parameters: 44 | use_sim_time: True 45 | 46 | amcl_rclcpp_node: 47 | ros__parameters: 48 | use_sim_time: True 49 | 50 | bt_navigator: 51 | ros__parameters: 52 | use_sim_time: True 53 | global_frame: map 54 | robot_base_frame: base_link 55 | odom_topic: /odom 56 | bt_loop_duration: 10 57 | default_server_timeout: 20 58 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 59 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 60 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 61 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 62 | # if enable_groot_monitoring is set to True, ports need to be different for each robot !! 63 | enable_groot_monitoring: False 64 | groot_zmq_publisher_port: 1789 65 | groot_zmq_server_port: 1887 66 | plugin_lib_names: 67 | - nav2_compute_path_to_pose_action_bt_node 68 | - nav2_compute_path_through_poses_action_bt_node 69 | - nav2_follow_path_action_bt_node 70 | - nav2_back_up_action_bt_node 71 | - nav2_spin_action_bt_node 72 | - nav2_wait_action_bt_node 73 | - nav2_clear_costmap_service_bt_node 74 | - nav2_is_stuck_condition_bt_node 75 | - nav2_goal_reached_condition_bt_node 76 | - nav2_goal_updated_condition_bt_node 77 | - nav2_initial_pose_received_condition_bt_node 78 | - nav2_reinitialize_global_localization_service_bt_node 79 | - nav2_rate_controller_bt_node 80 | - nav2_distance_controller_bt_node 81 | - nav2_speed_controller_bt_node 82 | - nav2_truncate_path_action_bt_node 83 | - nav2_goal_updater_node_bt_node 84 | - nav2_recovery_node_bt_node 85 | - nav2_pipeline_sequence_bt_node 86 | - nav2_round_robin_node_bt_node 87 | - nav2_transform_available_condition_bt_node 88 | - nav2_time_expired_condition_bt_node 89 | - nav2_distance_traveled_condition_bt_node 90 | - nav2_single_trigger_bt_node 91 | - nav2_is_battery_low_condition_bt_node 92 | - nav2_navigate_through_poses_action_bt_node 93 | - nav2_navigate_to_pose_action_bt_node 94 | - nav2_remove_passed_goals_action_bt_node 95 | - nav2_planner_selector_bt_node 96 | - nav2_controller_selector_bt_node 97 | - nav2_goal_checker_selector_bt_node 98 | 99 | bt_navigator_rclcpp_node: 100 | ros__parameters: 101 | use_sim_time: True 102 | 103 | controller_server: 104 | ros__parameters: 105 | use_sim_time: True 106 | controller_frequency: 20.0 107 | min_x_velocity_threshold: 0.001 108 | min_y_velocity_threshold: 0.5 109 | min_theta_velocity_threshold: 0.001 110 | progress_checker_plugin: "progress_checker" 111 | goal_checker_plugin: "goal_checker" 112 | controller_plugins: ["FollowPath"] 113 | 114 | # Progress checker parameters 115 | progress_checker: 116 | plugin: "nav2_controller::SimpleProgressChecker" 117 | required_movement_radius: 0.5 118 | movement_time_allowance: 10.0 119 | # Goal checker parameters 120 | goal_checker: 121 | plugin: "nav2_controller::SimpleGoalChecker" 122 | xy_goal_tolerance: 0.25 123 | yaw_goal_tolerance: 0.25 124 | stateful: True 125 | # DWB parameters 126 | FollowPath: 127 | plugin: "dwb_core::DWBLocalPlanner" 128 | debug_trajectory_details: True 129 | min_vel_x: 0.0 130 | min_vel_y: 0.0 131 | max_vel_x: 0.26 132 | max_vel_y: 0.0 133 | max_vel_theta: 1.0 134 | min_speed_xy: 0.0 135 | max_speed_xy: 0.26 136 | min_speed_theta: 0.0 137 | # Add high threshold velocity for turtlebot 3 issue. 138 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 139 | acc_lim_x: 2.5 140 | acc_lim_y: 0.0 141 | acc_lim_theta: 3.2 142 | decel_lim_x: -2.5 143 | decel_lim_y: 0.0 144 | decel_lim_theta: -3.2 145 | vx_samples: 20 146 | vy_samples: 5 147 | vtheta_samples: 20 148 | sim_time: 1.7 149 | linear_granularity: 0.05 150 | angular_granularity: 0.025 151 | transform_tolerance: 0.2 152 | trans_stopped_velocity: 0.25 153 | short_circuit_trajectory_evaluation: True 154 | stateful: True 155 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 156 | BaseObstacle.scale: 0.02 157 | PathAlign.scale: 0.0 158 | GoalAlign.scale: 0.0 159 | PathDist.scale: 32.0 160 | GoalDist.scale: 24.0 161 | RotateToGoal.scale: 32.0 162 | RotateToGoal.slowing_factor: 5.0 163 | RotateToGoal.lookahead_time: -1.0 164 | 165 | controller_server_rclcpp_node: 166 | ros__parameters: 167 | use_sim_time: True 168 | 169 | local_costmap: 170 | local_costmap: 171 | ros__parameters: 172 | use_sim_time: True 173 | global_frame: odom 174 | rolling_window: true 175 | width: 3 176 | height: 3 177 | resolution: 0.05 178 | robot_radius: 0.22 179 | plugins: ["obstacle_layer", "inflation_layer"] 180 | inflation_layer: 181 | plugin: "nav2_costmap_2d::InflationLayer" 182 | cost_scaling_factor: 3.0 183 | inflation_radius: 0.55 184 | obstacle_layer: 185 | plugin: "nav2_costmap_2d::ObstacleLayer" 186 | enabled: True 187 | scan: 188 | topic: /robot2/scan 189 | max_obstacle_height: 2.0 190 | clearing: True 191 | marking: True 192 | raytrace_max_range: 3.0 193 | raytrace_min_range: 0.0 194 | obstacle_max_range: 2.5 195 | obstacle_min_range: 0.0 196 | static_layer: 197 | map_subscribe_transient_local: True 198 | always_send_full_costmap: True 199 | observation_sources: scan 200 | local_costmap_client: 201 | ros__parameters: 202 | use_sim_time: True 203 | local_costmap_rclcpp_node: 204 | ros__parameters: 205 | use_sim_time: True 206 | 207 | global_costmap: 208 | global_costmap: 209 | ros__parameters: 210 | use_sim_time: True 211 | robot_radius: 0.22 212 | obstacle_layer: 213 | enabled: True 214 | scan: 215 | topic: /robot2/scan 216 | max_obstacle_height: 2.0 217 | clearing: True 218 | marking: True 219 | raytrace_max_range: 3.0 220 | raytrace_min_range: 0.0 221 | obstacle_max_range: 2.5 222 | obstacle_min_range: 0.0 223 | static_layer: 224 | map_subscribe_transient_local: True 225 | always_send_full_costmap: True 226 | observation_sources: scan 227 | global_costmap_client: 228 | ros__parameters: 229 | use_sim_time: True 230 | global_costmap_rclcpp_node: 231 | ros__parameters: 232 | use_sim_time: True 233 | 234 | map_server: 235 | ros__parameters: 236 | use_sim_time: True 237 | yaml_filename: "turtlebot3_world.yaml" 238 | save_map_timeout: 5.0 239 | 240 | planner_server: 241 | ros__parameters: 242 | use_sim_time: True 243 | planner_plugins: ["GridBased"] 244 | GridBased: 245 | plugin: "nav2_navfn_planner/NavfnPlanner" 246 | tolerance: 0.5 247 | use_astar: false 248 | allow_unknown: true 249 | 250 | planner_server_rclcpp_node: 251 | ros__parameters: 252 | use_sim_time: True 253 | 254 | recoveries_server: 255 | ros__parameters: 256 | costmap_topic: local_costmap/costmap_raw 257 | footprint_topic: local_costmap/published_footprint 258 | cycle_frequency: 10.0 259 | recovery_plugins: ["spin", "backup", "wait"] 260 | spin: 261 | plugin: "nav2_recoveries/Spin" 262 | backup: 263 | plugin: "nav2_recoveries/BackUp" 264 | wait: 265 | plugin: "nav2_recoveries/Wait" 266 | global_frame: odom 267 | robot_base_frame: base_link 268 | transform_timeout: 0.1 269 | use_sim_time: true 270 | simulate_ahead_time: 2.0 271 | max_rotational_vel: 1.0 272 | min_rotational_vel: 0.4 273 | rotational_acc_lim: 3.2 274 | 275 | robot_state_publisher: 276 | ros__parameters: 277 | use_sim_time: True 278 | 279 | waypoint_follower: 280 | ros__parameters: 281 | loop_rate: 20 282 | stop_on_failure: false 283 | waypoint_task_executor_plugin: "wait_at_waypoint" 284 | wait_at_waypoint: 285 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 286 | enabled: True 287 | waypoint_pause_duration: 200 -------------------------------------------------------------------------------- /map_merge/launch/tb3_simulation/slam_toolbox.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.substitutions import LaunchConfiguration 6 | from launch_ros.actions import Node 7 | from ament_index_python.packages import get_package_share_directory 8 | 9 | 10 | def generate_launch_description(): 11 | use_sim_time = LaunchConfiguration("use_sim_time") 12 | slam_params_file = LaunchConfiguration("slam_params_file") 13 | 14 | remappings = [ 15 | ("/map", "map"), 16 | ("/scan", "scan"), 17 | ("/tf", "tf"), 18 | ("/tf_static", "tf_static"), 19 | ] 20 | 21 | declare_use_sim_time_argument = DeclareLaunchArgument( 22 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 23 | ) 24 | declare_slam_params_file_cmd = DeclareLaunchArgument( 25 | "slam_params_file", 26 | default_value=os.path.join( 27 | get_package_share_directory("slam_toolbox"), 28 | "config", 29 | "mapper_params_online_sync.yaml", 30 | ), 31 | description="Full path to the ROS2 parameters file to use for the slam_toolbox node", 32 | ) 33 | 34 | start_sync_slam_toolbox_node = Node( 35 | parameters=[slam_params_file, {"use_sim_time": use_sim_time}], 36 | package="slam_toolbox", 37 | executable="sync_slam_toolbox_node", 38 | name="slam_toolbox", 39 | output="screen", 40 | remappings=remappings, 41 | ) 42 | 43 | ld = LaunchDescription() 44 | 45 | ld.add_action(declare_use_sim_time_argument) 46 | ld.add_action(declare_slam_params_file_cmd) 47 | ld.add_action(start_sync_slam_toolbox_node) 48 | 49 | return ld 50 | -------------------------------------------------------------------------------- /map_merge/launch/tb3_simulation/tb3_simulation_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """This is all-in-one launch script intended for use by nav2 developers.""" 16 | 17 | import os 18 | 19 | from ament_index_python.packages import get_package_share_directory 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import ( 23 | DeclareLaunchArgument, 24 | ExecuteProcess, 25 | IncludeLaunchDescription, 26 | ) 27 | from launch.conditions import IfCondition 28 | from launch.launch_description_sources import PythonLaunchDescriptionSource 29 | from launch.substitutions import LaunchConfiguration, PythonExpression 30 | from launch_ros.actions import Node 31 | 32 | 33 | def generate_launch_description(): 34 | # Get the launch directory 35 | bringup_dir = get_package_share_directory("nav2_bringup") 36 | launch_dir = os.path.join(bringup_dir, "launch") 37 | 38 | # Get the launch directory for multirobot_map_merge where we have a modified bringup launch file 39 | map_merge_dir = get_package_share_directory("multirobot_map_merge") 40 | launch_dir_map_merge = os.path.join(map_merge_dir, "launch", "tb3_simulation") 41 | 42 | # Create the launch configuration variables 43 | slam = LaunchConfiguration("slam") 44 | slam_toolbox = LaunchConfiguration("slam_toolbox") 45 | slam_gmapping = LaunchConfiguration("slam_gmapping") 46 | namespace = LaunchConfiguration("namespace") 47 | use_namespace = LaunchConfiguration("use_namespace") 48 | map_yaml_file = LaunchConfiguration("map") 49 | use_sim_time = LaunchConfiguration("use_sim_time") 50 | params_file = LaunchConfiguration("params_file") 51 | autostart = LaunchConfiguration("autostart") 52 | 53 | # Launch configuration variables specific to simulation 54 | rviz_config_file = LaunchConfiguration("rviz_config_file") 55 | use_simulator = LaunchConfiguration("use_simulator") 56 | use_robot_state_pub = LaunchConfiguration("use_robot_state_pub") 57 | use_rviz = LaunchConfiguration("use_rviz") 58 | headless = LaunchConfiguration("headless") 59 | world = LaunchConfiguration("world") 60 | 61 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 62 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 63 | # https://github.com/ros/geometry2/issues/32 64 | # https://github.com/ros/robot_state_publisher/pull/30 65 | # TODO(orduno) Substitute with `PushNodeRemapping` 66 | # https://github.com/ros2/launch_ros/issues/56 67 | remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] 68 | 69 | # Declare the launch arguments 70 | declare_namespace_cmd = DeclareLaunchArgument( 71 | "namespace", default_value="", description="Top-level namespace" 72 | ) 73 | 74 | declare_use_namespace_cmd = DeclareLaunchArgument( 75 | "use_namespace", 76 | default_value="false", 77 | description="Whether to apply a namespace to the navigation stack", 78 | ) 79 | 80 | declare_slam_cmd = DeclareLaunchArgument( 81 | "slam", default_value="False", description="Whether run a SLAM" 82 | ) 83 | declare_slam_toolbox_cmd = DeclareLaunchArgument( 84 | "slam_toolbox", default_value="False", description="Whether run a SLAM toolbox" 85 | ) 86 | declare_slam_gmapping_cmd = DeclareLaunchArgument( 87 | "slam_gmapping", 88 | default_value="False", 89 | description="Whether run a SLAM gmapping", 90 | ) 91 | 92 | declare_map_yaml_cmd = DeclareLaunchArgument( 93 | "map", 94 | default_value=os.path.join(bringup_dir, "maps", "turtlebot3_world.yaml"), 95 | description="Full path to map file to load", 96 | ) 97 | 98 | declare_use_sim_time_cmd = DeclareLaunchArgument( 99 | "use_sim_time", 100 | default_value="true", 101 | description="Use simulation (Gazebo) clock if true", 102 | ) 103 | 104 | declare_params_file_cmd = DeclareLaunchArgument( 105 | "params_file", 106 | default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"), 107 | description="Full path to the ROS2 parameters file to use for all launched nodes", 108 | ) 109 | 110 | declare_autostart_cmd = DeclareLaunchArgument( 111 | "autostart", 112 | default_value="true", 113 | description="Automatically startup the nav2 stack", 114 | ) 115 | 116 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 117 | "rviz_config_file", 118 | default_value=os.path.join(bringup_dir, "rviz", "nav2_default_view.rviz"), 119 | description="Full path to the RVIZ config file to use", 120 | ) 121 | 122 | declare_use_simulator_cmd = DeclareLaunchArgument( 123 | "use_simulator", 124 | default_value="True", 125 | description="Whether to start the simulator", 126 | ) 127 | 128 | declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 129 | "use_robot_state_pub", 130 | default_value="True", 131 | description="Whether to start the robot state publisher", 132 | ) 133 | 134 | declare_use_rviz_cmd = DeclareLaunchArgument( 135 | "use_rviz", default_value="True", description="Whether to start RVIZ" 136 | ) 137 | 138 | declare_simulator_cmd = DeclareLaunchArgument( 139 | "headless", default_value="False", description="Whether to execute gzclient)" 140 | ) 141 | 142 | declare_world_cmd = DeclareLaunchArgument( 143 | "world", 144 | # TODO(orduno) Switch back once ROS argument passing has been fixed upstream 145 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91 146 | # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), 147 | # worlds/turtlebot3_worlds/waffle.model') 148 | default_value=os.path.join(bringup_dir, "worlds", "waffle.model"), 149 | description="Full path to world model file to load", 150 | ) 151 | 152 | # Specify the actions 153 | start_gazebo_server_cmd = ExecuteProcess( 154 | condition=IfCondition(use_simulator), 155 | cmd=[ 156 | "gzserver", 157 | "-s", 158 | "libgazebo_ros_init.so", 159 | "-s", 160 | "libgazebo_ros_factory.so", 161 | world, 162 | ], 163 | cwd=[launch_dir], 164 | output="screen", 165 | ) 166 | 167 | start_gazebo_client_cmd = ExecuteProcess( 168 | condition=IfCondition(PythonExpression([use_simulator, " and not ", headless])), 169 | cmd=["gzclient"], 170 | cwd=[launch_dir], 171 | output="screen", 172 | ) 173 | 174 | urdf = os.path.join(bringup_dir, "urdf", "turtlebot3_waffle.urdf") 175 | 176 | start_robot_state_publisher_cmd = Node( 177 | condition=IfCondition(use_robot_state_pub), 178 | package="robot_state_publisher", 179 | executable="robot_state_publisher", 180 | name="robot_state_publisher", 181 | namespace=namespace, 182 | output="screen", 183 | parameters=[{"use_sim_time": use_sim_time}], 184 | remappings=remappings, 185 | arguments=[urdf], 186 | ) 187 | 188 | rviz_cmd = IncludeLaunchDescription( 189 | PythonLaunchDescriptionSource(os.path.join(launch_dir, "rviz_launch.py")), 190 | condition=IfCondition(use_rviz), 191 | launch_arguments={ 192 | "namespace": "", 193 | "use_namespace": "False", 194 | "rviz_config": rviz_config_file, 195 | }.items(), 196 | ) 197 | 198 | bringup_cmd = IncludeLaunchDescription( 199 | PythonLaunchDescriptionSource( 200 | os.path.join(launch_dir_map_merge, "bringup_launch.py") 201 | ), 202 | launch_arguments={ 203 | "namespace": namespace, 204 | "use_namespace": use_namespace, 205 | "slam": slam, 206 | "slam_toolbox": slam_toolbox, 207 | "slam_gmapping": slam_gmapping, 208 | "map": map_yaml_file, 209 | "use_sim_time": use_sim_time, 210 | "params_file": params_file, 211 | "autostart": autostart, 212 | }.items(), 213 | ) 214 | 215 | # Create the launch description and populate 216 | ld = LaunchDescription() 217 | 218 | # Declare the launch options 219 | ld.add_action(declare_namespace_cmd) 220 | ld.add_action(declare_use_namespace_cmd) 221 | ld.add_action(declare_slam_cmd) 222 | ld.add_action(declare_slam_toolbox_cmd) 223 | ld.add_action(declare_slam_gmapping_cmd) 224 | ld.add_action(declare_map_yaml_cmd) 225 | ld.add_action(declare_use_sim_time_cmd) 226 | ld.add_action(declare_params_file_cmd) 227 | ld.add_action(declare_autostart_cmd) 228 | 229 | ld.add_action(declare_rviz_config_file_cmd) 230 | ld.add_action(declare_use_simulator_cmd) 231 | ld.add_action(declare_use_robot_state_pub_cmd) 232 | ld.add_action(declare_use_rviz_cmd) 233 | ld.add_action(declare_simulator_cmd) 234 | ld.add_action(declare_world_cmd) 235 | 236 | # Add any conditioned actions 237 | ld.add_action(start_gazebo_server_cmd) 238 | ld.add_action(start_gazebo_client_cmd) 239 | 240 | # Add the actions to launch all of the navigation nodes 241 | ld.add_action(start_robot_state_publisher_cmd) 242 | ld.add_action(rviz_cmd) 243 | ld.add_action(bringup_cmd) 244 | 245 | return ld 246 | -------------------------------------------------------------------------------- /map_merge/launch/tb3_simulation/worlds/world_only.model: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | false 15 | 16 | 17 | 18 | 19 | 0.319654 -0.235002 9.29441 0 1.5138 0.009599 20 | orbit 21 | perspective 22 | 23 | 24 | 25 | 26 | 1000.0 27 | 0.001 28 | 1 29 | 30 | 31 | quick 32 | 150 33 | 0 34 | 1.400000 35 | 1 36 | 37 | 38 | 0.00001 39 | 0.2 40 | 2000.000000 41 | 0.01000 42 | 43 | 44 | 45 | 46 | 47 | 1 48 | 49 | 50 | 51 | model://turtlebot3_house 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /map_merge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | multirobot_map_merge 5 | 1.0.0 6 | 7 | Merging multiple maps without knowledge of initial 8 | positions of robots ROS2 Port. 9 | 10 | Carlos Alvarez 11 | Carlos Alvarez 12 | BSD 13 | 14 | ament_cmake 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | ament_cmake 19 | geometry_msgs 20 | nav_msgs 21 | map_msgs 22 | tf2_geometry_msgs 23 | tf2 24 | tf2_ros 25 | 26 | image_geometry 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /map_merge/src/combine_grids/estimation_internal.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * Copyright (c) 2021, Carlos Alvarez. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #ifndef ESTIMATION_INTERNAL_H_ 39 | #define ESTIMATION_INTERNAL_H_ 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #ifdef HAVE_OPENCV_XFEATURES2D 52 | #include 53 | #endif 54 | 55 | namespace combine_grids 56 | { 57 | namespace internal 58 | { 59 | #if CV_VERSION_MAJOR >= 4 60 | 61 | static inline cv::Ptr chooseFeatureFinder(FeatureType type) 62 | { 63 | switch (type) { 64 | case FeatureType::AKAZE: 65 | return cv::AKAZE::create(); 66 | case FeatureType::ORB: 67 | return cv::ORB::create(); 68 | case FeatureType::SURF: 69 | #ifdef HAVE_OPENCV_XFEATURES2D 70 | return cv::xfeatures2d::SURF::create(); 71 | #else 72 | return cv::AKAZE::create(); 73 | #endif 74 | } 75 | 76 | assert(false); 77 | return {}; 78 | } 79 | 80 | #else // (CV_VERSION_MAJOR < 4) 81 | 82 | static inline cv::Ptr 83 | chooseFeatureFinder(FeatureType type) 84 | { 85 | switch (type) { 86 | case FeatureType::AKAZE: 87 | return cv::makePtr(); 88 | case FeatureType::ORB: 89 | return cv::makePtr(); 90 | case FeatureType::SURF: 91 | return cv::makePtr(); 92 | } 93 | 94 | assert(false); 95 | return {}; 96 | } 97 | 98 | #endif // CV_VERSION_MAJOR >= 4 99 | 100 | static inline void writeDebugMatchingInfo( 101 | const std::vector& images, 102 | const std::vector& image_features, 103 | const std::vector& pairwise_matches) 104 | { 105 | for (auto& match_info : pairwise_matches) { 106 | if (match_info.H.empty() || 107 | match_info.src_img_idx >= match_info.dst_img_idx) { 108 | continue; 109 | } 110 | std::cout << match_info.src_img_idx << " " << match_info.dst_img_idx 111 | << std::endl 112 | << "features: " 113 | << image_features[size_t(match_info.src_img_idx)].keypoints.size() 114 | << " " 115 | << image_features[size_t(match_info.dst_img_idx)].keypoints.size() 116 | << std::endl 117 | << "matches: " << match_info.matches.size() << std::endl 118 | << "inliers: " << match_info.num_inliers << std::endl 119 | << "inliers/matches ratio: " 120 | << match_info.num_inliers / double(match_info.matches.size()) 121 | << std::endl 122 | << "confidence: " << match_info.confidence << std::endl 123 | << match_info.H << std::endl; 124 | cv::Mat img; 125 | // draw all matches 126 | cv::drawMatches(images[size_t(match_info.src_img_idx)], 127 | image_features[size_t(match_info.src_img_idx)].keypoints, 128 | images[size_t(match_info.dst_img_idx)], 129 | image_features[size_t(match_info.dst_img_idx)].keypoints, 130 | match_info.matches, img); 131 | cv::imwrite(std::to_string(match_info.src_img_idx) + "_" + 132 | std::to_string(match_info.dst_img_idx) + "_matches.png", 133 | img); 134 | // draw inliers only 135 | cv::drawMatches( 136 | images[size_t(match_info.src_img_idx)], 137 | image_features[size_t(match_info.src_img_idx)].keypoints, 138 | images[size_t(match_info.dst_img_idx)], 139 | image_features[size_t(match_info.dst_img_idx)].keypoints, 140 | match_info.matches, img, cv::Scalar::all(-1), cv::Scalar::all(-1), 141 | *reinterpret_cast*>(&match_info.inliers_mask)); 142 | cv::imwrite(std::to_string(match_info.src_img_idx) + "_" + 143 | std::to_string(match_info.dst_img_idx) + 144 | "_matches_inliers.png", 145 | img); 146 | } 147 | } 148 | 149 | } // namespace internal 150 | } // namespace combine_grids 151 | 152 | #endif // ESTIMATION_INTERNAL_H_ 153 | -------------------------------------------------------------------------------- /map_merge/src/combine_grids/grid_compositor.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * Copyright (c) 2021, Carlos Alvarez. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | 44 | 45 | namespace combine_grids 46 | { 47 | namespace internal 48 | { 49 | nav_msgs::msg::OccupancyGrid::SharedPtr GridCompositor::compose( 50 | const std::vector& grids, const std::vector& rois) 51 | { 52 | rcpputils::require_true(grids.size() == rois.size()); 53 | 54 | nav_msgs::msg::OccupancyGrid::SharedPtr result_grid(new nav_msgs::msg::OccupancyGrid()); 55 | 56 | std::vector corners; 57 | corners.reserve(grids.size()); 58 | std::vector sizes; 59 | sizes.reserve(grids.size()); 60 | for (auto& roi : rois) { 61 | corners.push_back(roi.tl()); 62 | sizes.push_back(roi.size()); 63 | } 64 | cv::Rect dst_roi = cv::detail::resultRoi(corners, sizes); 65 | 66 | result_grid->info.width = static_cast(dst_roi.width); 67 | result_grid->info.height = static_cast(dst_roi.height); 68 | result_grid->data.resize(static_cast(dst_roi.area()), -1); 69 | // create view for opencv pointing to newly allocated grid 70 | cv::Mat result(dst_roi.size(), CV_8S, result_grid->data.data()); 71 | 72 | for (size_t i = 0; i < grids.size(); ++i) { 73 | // we need to compensate global offset 74 | cv::Rect roi = cv::Rect(corners[i] - dst_roi.tl(), sizes[i]); 75 | cv::Mat result_roi(result, roi); 76 | // reinterpret warped matrix as signed 77 | // we will not change this matrix, but opencv does not support const matrices 78 | cv::Mat warped_signed (grids[i].size(), CV_8S, const_cast(grids[i].ptr())); 79 | // compose img into result matrix 80 | cv::max(result_roi, warped_signed, result_roi); 81 | } 82 | 83 | return result_grid; 84 | } 85 | 86 | } // namespace internal 87 | 88 | } // namespace combine_grids 89 | -------------------------------------------------------------------------------- /map_merge/src/combine_grids/grid_warper.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * Copyright (c) 2021, Carlos Alvarez. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | 44 | namespace combine_grids 45 | { 46 | namespace internal 47 | { 48 | cv::Rect GridWarper::warp(const cv::Mat& grid, const cv::Mat& transform, 49 | cv::Mat& warped_grid) 50 | { 51 | // for validating function inputs. Throws an std::invalid_argument exception if the condition fails. 52 | rcpputils::require_true(transform.type() == CV_64F); 53 | cv::Mat H; 54 | invertAffineTransform(transform.rowRange(0, 2), H); 55 | cv::Rect roi = warpRoi(grid, H); 56 | // shift top left corner for warp affine (otherwise the image is cropped) 57 | H.at(0, 2) -= roi.tl().x; 58 | H.at(1, 2) -= roi.tl().y; 59 | warpAffine(grid, warped_grid, H, roi.size(), cv::INTER_NEAREST, 60 | cv::BORDER_CONSTANT, 61 | cv::Scalar::all(255) /* this is -1 for signed char */); 62 | 63 | // For verifying results. Throws a rcpputils::AssertionException if the condition fails. 64 | // This function becomes a no-op in release builds. 65 | rcpputils::assert_true(roi.size() == warped_grid.size()); 66 | 67 | return roi; 68 | } 69 | 70 | cv::Rect GridWarper::warpRoi(const cv::Mat& grid, const cv::Mat& transform) 71 | { 72 | cv::Ptr warper = 73 | cv::makePtr(); 74 | cv::Mat H; 75 | transform.convertTo(H, CV_32F); 76 | 77 | // separate rotation and translation for plane warper 78 | // 3D translation 79 | cv::Mat T = cv::Mat::zeros(3, 1, CV_32F); 80 | H.colRange(2, 3).rowRange(0, 2).copyTo(T.rowRange(0, 2)); 81 | // 3D rotation 82 | cv::Mat R = cv::Mat::eye(3, 3, CV_32F); 83 | H.colRange(0, 2).copyTo(R.rowRange(0, 2).colRange(0, 2)); 84 | 85 | return warper->warpRoi(grid.size(), cv::Mat::eye(3, 3, CV_32F), R, T); 86 | } 87 | 88 | } // namespace internal 89 | 90 | } // namespace combine_grids 91 | -------------------------------------------------------------------------------- /map_merge/src/combine_grids/merging_pipeline.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * Copyright (c) 2021, Carlos Alvarez. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | #include "estimation_internal.h" 48 | 49 | namespace combine_grids 50 | { 51 | bool MergingPipeline::estimateTransforms(FeatureType feature_type, 52 | double confidence) 53 | { 54 | std::vector image_features; 55 | std::vector pairwise_matches; 56 | std::vector transforms; 57 | std::vector good_indices; 58 | // TODO investigate value translation effect on features 59 | auto finder = internal::chooseFeatureFinder(feature_type); 60 | cv::Ptr matcher = 61 | cv::makePtr(); 62 | cv::Ptr estimator = 63 | cv::makePtr(); 64 | cv::Ptr adjuster = 65 | cv::makePtr(); 66 | 67 | if (images_.empty()) { 68 | return true; 69 | } 70 | 71 | /* find features in images */ 72 | static rclcpp::Logger logger = rclcpp::get_logger("estimateTransforms"); 73 | RCLCPP_DEBUG(logger, "computing features"); 74 | image_features.reserve(images_.size()); 75 | for (const cv::Mat& image : images_) { 76 | image_features.emplace_back(); 77 | if (!image.empty()) { 78 | #if CV_VERSION_MAJOR >= 4 79 | cv::detail::computeImageFeatures(finder, image, image_features.back()); 80 | #else 81 | (*finder)(image, image_features.back()); 82 | #endif 83 | } 84 | } 85 | finder = {}; 86 | 87 | /* find corespondent features */ 88 | RCLCPP_DEBUG(logger, "pairwise matching features"); 89 | (*matcher)(image_features, pairwise_matches); 90 | matcher = {}; 91 | 92 | #ifndef NDEBUG 93 | internal::writeDebugMatchingInfo(images_, image_features, pairwise_matches); 94 | #endif 95 | 96 | /* use only matches that has enough confidence. leave out matches that are not 97 | * connected (small components) */ 98 | good_indices = cv::detail::leaveBiggestComponent( 99 | image_features, pairwise_matches, static_cast(confidence)); 100 | 101 | // no match found. try set first non-empty grid as reference frame. we try to 102 | // avoid setting empty grid as reference frame, in case some maps never 103 | // arrive. If all is empty just set null transforms. 104 | if (good_indices.size() == 1) { 105 | transforms_.clear(); 106 | transforms_.resize(images_.size()); 107 | 108 | // Making some tests to see if it is better to just return false if no match is found 109 | // and not clear the last good transforms found 110 | // if (images_.size() != transforms_.size()) { 111 | // transforms_.clear(); 112 | // transforms_.resize(images_.size()); 113 | // } 114 | // return false; 115 | 116 | for (size_t i = 0; i < images_.size(); ++i) { 117 | if (!images_[i].empty()) { 118 | // set identity 119 | transforms_[i] = cv::Mat::eye(3, 3, CV_64F); 120 | break; 121 | } 122 | } 123 | // RCLCPP_INFO(logger, "No match found between maps, setting first non-empty grid as reference frame"); 124 | return true; 125 | } 126 | 127 | // // Experimental: should we keep only the best confidence match overall? 128 | // bool max_confidence_achieved_surpassed = false; 129 | // for (auto &match_info : pairwise_matches) { 130 | // RCLCPP_INFO(logger, "match info: %f", match_info.confidence); 131 | // if (match_info.confidence > max_confidence_achieved_){ 132 | // max_confidence_achieved_surpassed = true; 133 | // max_confidence_achieved_ = match_info.confidence; 134 | // } 135 | // } 136 | // if (!max_confidence_achieved_surpassed) { 137 | // RCLCPP_INFO(logger, "Max confidence achieved not surpassed, not using matching"); 138 | // return false; 139 | // } 140 | // else 141 | // RCLCPP_INFO(logger, "Max confidence achieved surpassed, optimizing"); 142 | 143 | 144 | /* estimate transform */ 145 | RCLCPP_DEBUG(logger, "calculating transforms in global reference frame"); 146 | // note: currently used estimator never fails 147 | if (!(*estimator)(image_features, pairwise_matches, transforms)) { 148 | return false; 149 | } 150 | 151 | /* levmarq optimization */ 152 | // openCV just accepts float transforms 153 | for (auto& transform : transforms) { 154 | transform.R.convertTo(transform.R, CV_32F); 155 | } 156 | RCLCPP_DEBUG(logger, "optimizing global transforms"); 157 | adjuster->setConfThresh(confidence); 158 | if (!(*adjuster)(image_features, pairwise_matches, transforms)) { 159 | RCLCPP_WARN(logger, "Bundle adjusting failed. Could not estimate transforms."); 160 | return false; 161 | } 162 | 163 | transforms_.clear(); 164 | transforms_.resize(images_.size()); 165 | size_t i = 0; 166 | for (auto& j : good_indices) { 167 | // we want to work with transforms as doubles 168 | transforms[i].R.convertTo(transforms_[static_cast(j)], CV_64F); 169 | ++i; 170 | } 171 | 172 | return true; 173 | } 174 | 175 | // checks whether given matrix is an identity, i.e. exactly appropriate Mat::eye 176 | static inline bool isIdentity(const cv::Mat& matrix) 177 | { 178 | if (matrix.empty()) { 179 | return false; 180 | } 181 | cv::MatExpr diff = matrix != cv::Mat::eye(matrix.size(), matrix.type()); 182 | return cv::countNonZero(diff) == 0; 183 | } 184 | 185 | nav_msgs::msg::OccupancyGrid::SharedPtr MergingPipeline::composeGrids() 186 | { 187 | // for checking states. Throws a rcpputils::IllegalStateException if the condition fails. 188 | rcpputils::check_true(images_.size() == transforms_.size()); 189 | rcpputils::check_true(images_.size() == grids_.size()); 190 | static rclcpp::Logger logger = rclcpp::get_logger("composeGrids"); 191 | 192 | if (images_.empty()) { 193 | return nullptr; 194 | } 195 | 196 | RCLCPP_DEBUG(logger, "warping grids"); 197 | internal::GridWarper warper; 198 | std::vector imgs_warped; 199 | imgs_warped.reserve(images_.size()); 200 | std::vector rois; 201 | rois.reserve(images_.size()); 202 | 203 | for (size_t i = 0; i < images_.size(); ++i) { 204 | if (!transforms_[i].empty() && !images_[i].empty()) { 205 | imgs_warped.emplace_back(); 206 | rois.emplace_back( 207 | warper.warp(images_[i], transforms_[i], imgs_warped.back())); 208 | } 209 | } 210 | 211 | if (imgs_warped.empty()) { 212 | return nullptr; 213 | } 214 | 215 | RCLCPP_DEBUG(logger, "compositing result grid"); 216 | nav_msgs::msg::OccupancyGrid::SharedPtr result; 217 | internal::GridCompositor compositor; 218 | result = compositor.compose(imgs_warped, rois); 219 | 220 | // set correct resolution to output grid. use resolution of identity (works 221 | // for estimated transforms), or any resolution (works for know_init_positions) 222 | // - in that case all resolutions should be the same. 223 | float any_resolution = 0.0; 224 | for (size_t i = 0; i < transforms_.size(); ++i) { 225 | // check if this transform is the reference frame 226 | if (isIdentity(transforms_[i])) { 227 | result->info.resolution = grids_[i]->info.resolution; 228 | break; 229 | } 230 | if (grids_[i]) { 231 | any_resolution = grids_[i]->info.resolution; 232 | } 233 | } 234 | if (result->info.resolution <= 0.f) { 235 | result->info.resolution = any_resolution; 236 | } 237 | 238 | // set grid origin to its centre 239 | result->info.origin.position.x = 240 | -(result->info.width / 2.0) * double(result->info.resolution); 241 | result->info.origin.position.y = 242 | -(result->info.height / 2.0) * double(result->info.resolution); 243 | result->info.origin.orientation.w = 1.0; 244 | 245 | return result; 246 | } 247 | 248 | std::vector MergingPipeline::getTransforms() const 249 | { 250 | std::vector result; 251 | result.reserve(transforms_.size()); 252 | 253 | for (auto& transform : transforms_) { 254 | if (transform.empty()) { 255 | result.emplace_back(); 256 | continue; 257 | } 258 | 259 | rcpputils::require_true(transform.type() == CV_64F); 260 | geometry_msgs::msg::Transform ros_transform; 261 | ros_transform.translation.x = transform.at(0, 2); 262 | ros_transform.translation.y = transform.at(1, 2); 263 | ros_transform.translation.z = 0.; 264 | 265 | // our rotation is in fact only 2D, thus quaternion can be simplified 266 | double a = transform.at(0, 0); 267 | double b = transform.at(1, 0); 268 | ros_transform.rotation.w = std::sqrt(2. + 2. * a) * 0.5; 269 | ros_transform.rotation.x = 0.; 270 | ros_transform.rotation.y = 0.; 271 | ros_transform.rotation.z = std::copysign(std::sqrt(2. - 2. * a) * 0.5, b); 272 | 273 | result.push_back(ros_transform); 274 | } 275 | 276 | return result; 277 | } 278 | 279 | } // namespace combine_grids 280 | -------------------------------------------------------------------------------- /map_merge/test/download.sh: -------------------------------------------------------------------------------- 1 | file_name=$1 2 | url=$2 3 | md5=$3 4 | 5 | # Download the file if it doesn't exist 6 | if [ ! -f $file_name ]; then 7 | wget $url -O $file_name 8 | fi 9 | 10 | # Check the MD5 sum of the file 11 | echo "Checking MD5 sum of $file_name" 12 | md5sum -c <<<"$md5 $file_name" 13 | if [ $? -ne 0 ]; then 14 | echo "MD5 sum of $file_name does not match. Downloading it again" 15 | wget $url -O $file_name 16 | md5sum -c <<<"$md5 $file_name" 17 | if [ $? -ne 0 ]; then 18 | echo "MD5 sum of $file_name still does not match. Aborting." 19 | exit 1 20 | fi 21 | fi -------------------------------------------------------------------------------- /map_merge/test/download_data.sh: -------------------------------------------------------------------------------- 1 | base_url=https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge 2 | wget ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm -P build/multirobot_map_merge 3 | wget ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm -P build/multirobot_map_merge 4 | wget ${base_url}/hector_maps/map05.pgm -P build/multirobot_map_merge 5 | wget ${base_url}/hector_maps/map00.pgm -P build/multirobot_map_merge -------------------------------------------------------------------------------- /map_merge/test/test_merging_pipeline.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * Copyright (c) 2022, Carlos Alvarez. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #include 39 | #include 40 | // #include 41 | #include 42 | #include "testing_helpers.h" 43 | 44 | #define private public 45 | #include 46 | 47 | const std::array hector_maps = { 48 | "map00.pgm", 49 | "map05.pgm", 50 | }; 51 | 52 | const std::array gmapping_maps = { 53 | "2011-08-09-12-22-52.pgm", 54 | "2012-01-28-11-12-01.pgm", 55 | }; 56 | 57 | constexpr bool verbose_tests = false; 58 | 59 | #define EXPECT_VALID_GRID(grid) \ 60 | ASSERT_TRUE(static_cast(grid)); \ 61 | EXPECT_TRUE(consistentData(*grid)); \ 62 | EXPECT_GT(grid->info.resolution, 0); \ 63 | EXPECT_TRUE(isIdentity(grid->info.origin.orientation)) 64 | 65 | TEST(MergingPipeline, canStich0Grid) 66 | { 67 | std::vector maps; 68 | combine_grids::MergingPipeline merger; 69 | merger.feed(maps.begin(), maps.end()); 70 | EXPECT_TRUE(merger.estimateTransforms()); 71 | EXPECT_EQ(merger.composeGrids(), nullptr); 72 | EXPECT_EQ(merger.getTransforms().size(), (long unsigned int) 0); 73 | } 74 | 75 | TEST(MergingPipeline, canStich1Grid) 76 | { 77 | auto map = loadMap(hector_maps[1]); 78 | combine_grids::MergingPipeline merger; 79 | merger.feed(&map, &map + 1); 80 | merger.estimateTransforms(); 81 | auto merged_grid = merger.composeGrids(); 82 | 83 | EXPECT_VALID_GRID(merged_grid); 84 | // don't use EXPECT_EQ, since it prints too much info 85 | EXPECT_TRUE(maps_equal(*merged_grid, *map)); 86 | 87 | // check estimated transforms 88 | auto transforms = merger.getTransforms(); 89 | EXPECT_EQ(transforms.size(), (long unsigned int) 1); 90 | EXPECT_TRUE(isIdentity(transforms[0])); 91 | } 92 | 93 | TEST(MergingPipeline, canStich2Grids) 94 | { 95 | auto maps = loadMaps(hector_maps.begin(), hector_maps.end()); 96 | combine_grids::MergingPipeline merger; 97 | merger.feed(maps.begin(), maps.end()); 98 | merger.estimateTransforms(); 99 | auto merged_grid = merger.composeGrids(); 100 | 101 | EXPECT_VALID_GRID(merged_grid); 102 | // grid size should indicate sucessful merge 103 | EXPECT_NEAR(2091, merged_grid->info.width, 30); 104 | EXPECT_NEAR(2091, merged_grid->info.height, 30); 105 | 106 | if (verbose_tests) { 107 | saveMap("test_canStich2Grids.pgm", merged_grid); 108 | } 109 | } 110 | 111 | TEST(MergingPipeline, canStichGridsGmapping) 112 | { 113 | auto maps = loadMaps(gmapping_maps.begin(), gmapping_maps.end()); 114 | combine_grids::MergingPipeline merger; 115 | merger.feed(maps.begin(), maps.end()); 116 | merger.estimateTransforms(); 117 | auto merged_grid = merger.composeGrids(); 118 | 119 | EXPECT_VALID_GRID(merged_grid); 120 | // grid size should indicate sucessful merge 121 | EXPECT_NEAR(5427, merged_grid->info.width, 30); 122 | EXPECT_NEAR(5427, merged_grid->info.height, 30); 123 | 124 | if (verbose_tests) { 125 | saveMap("canStichGridsGmapping.pgm", merged_grid); 126 | } 127 | } 128 | 129 | TEST(MergingPipeline, estimationAccuracy) 130 | { 131 | // for this test we measure estimation on the same grid artificially 132 | // transformed 133 | double angle = 0.523599 /* 30 deg in rads*/; 134 | // TODO test also translations 135 | double tx = 0; 136 | double ty = 0; 137 | cv::Matx23d transform{std::cos(angle), -std::sin(angle), tx, 138 | std::sin(angle), std::cos(angle), ty}; 139 | 140 | auto map = loadMap(hector_maps[1]); 141 | combine_grids::MergingPipeline merger; 142 | merger.feed(&map, &map + 1); 143 | 144 | // warp the map with Affine Transform 145 | combine_grids::internal::GridWarper warper; 146 | cv::Mat warped; 147 | auto roi = warper.warp(merger.images_[0], cv::Mat(transform), warped); 148 | 149 | // add warped map 150 | // this relies on internal implementation of merging pipeline 151 | merger.grids_.emplace_back(); 152 | merger.images_.push_back(warped); 153 | 154 | merger.estimateTransforms(); 155 | auto merged_grid = merger.composeGrids(); 156 | 157 | EXPECT_VALID_GRID(merged_grid); 158 | // transforms 159 | auto transforms = merger.getTransforms(); 160 | EXPECT_EQ(transforms.size(), (long unsigned int) 2); 161 | EXPECT_TRUE(isIdentity(transforms[0])); 162 | tf2::Transform t; 163 | tf2::fromMsg(transforms[1], t); 164 | 165 | EXPECT_NEAR(angle, t.getRotation().getAngle(), 1e-2); 166 | EXPECT_NEAR(tx - roi.tl().x, t.getOrigin().x(), 2); 167 | EXPECT_NEAR(ty - roi.tl().y, t.getOrigin().y(), 2); 168 | } 169 | 170 | TEST(MergingPipeline, transformsRoundTrip) 171 | { 172 | auto map = loadMap(hector_maps[0]); 173 | combine_grids::MergingPipeline merger; 174 | merger.feed(&map, &map + 1); 175 | for (size_t i = 0; i < 1000; ++i) { 176 | auto in_transform = randomTransform(); 177 | merger.setTransforms(&in_transform, &in_transform + 1); 178 | 179 | auto out_transforms = merger.getTransforms(); 180 | ASSERT_EQ(out_transforms.size(), (long unsigned int) 1); 181 | auto out_transform = out_transforms[0]; 182 | EXPECT_FLOAT_EQ(in_transform.translation.x, out_transform.translation.x); 183 | EXPECT_FLOAT_EQ(in_transform.translation.y, out_transform.translation.y); 184 | EXPECT_FLOAT_EQ(in_transform.translation.z, out_transform.translation.z); 185 | EXPECT_FLOAT_EQ(in_transform.rotation.x, out_transform.rotation.x); 186 | EXPECT_FLOAT_EQ(in_transform.rotation.y, out_transform.rotation.y); 187 | EXPECT_FLOAT_EQ(in_transform.rotation.z, out_transform.rotation.z); 188 | EXPECT_FLOAT_EQ(in_transform.rotation.w, out_transform.rotation.w); 189 | } 190 | } 191 | 192 | TEST(MergingPipeline, setTransformsInternal) 193 | { 194 | auto map = loadMap(hector_maps[0]); 195 | combine_grids::MergingPipeline merger; 196 | merger.feed(&map, &map + 1); 197 | 198 | for (size_t i = 0; i < 1000; ++i) { 199 | auto transform = randomTransform(); 200 | merger.setTransforms(&transform, &transform + 1); 201 | 202 | ASSERT_EQ(merger.transforms_.size(), (long unsigned int) 1); 203 | auto& transform_internal = merger.transforms_[0]; 204 | // verify that transforms are the same in 2D 205 | tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}}; 206 | cv::Point3d b[2] = {{1., 0., 1.}, {0., 1., 1.}}; 207 | for (auto j : {0, 1}) { 208 | tf2::Transform t; 209 | fromMsg(transform, t); 210 | auto p1 = t * a[j]; 211 | cv::Mat p2 = transform_internal * cv::Mat(b[j]); 212 | // some precision is naturally lost during conversion, float precision is 213 | // still good for us 214 | EXPECT_FLOAT_EQ(p1.x(), p2.at(0)); 215 | EXPECT_FLOAT_EQ(p1.y(), p2.at(1)); 216 | } 217 | } 218 | } 219 | 220 | TEST(MergingPipeline, getTransformsInternal) 221 | { 222 | auto map = loadMap(hector_maps[0]); 223 | combine_grids::MergingPipeline merger; 224 | merger.feed(&map, &map + 1); 225 | 226 | // set internal transform 227 | merger.transforms_.resize(1); 228 | for (size_t i = 0; i < 1000; ++i) { 229 | cv::Mat transform_internal = randomTransformMatrix(); 230 | merger.transforms_[0] = transform_internal; 231 | auto transforms = merger.getTransforms(); 232 | ASSERT_EQ(transforms.size(), (long unsigned int) 1); 233 | // output quaternion should be normalized 234 | auto& q = transforms[0].rotation; 235 | EXPECT_DOUBLE_EQ(1., q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); 236 | 237 | // verify that transforms are the same in 2D 238 | tf2::Transform transform; 239 | fromMsg(transforms[0], transform); 240 | tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}}; 241 | cv::Point3d b[2] = {{1., 0., 1.}, {0., 1., 1.}}; 242 | for (auto j : {0, 1}) { 243 | auto p1 = transform * a[j]; 244 | cv::Mat p2 = transform_internal * cv::Mat(b[j]); 245 | EXPECT_FLOAT_EQ(p1.x(), p2.at(0)); 246 | EXPECT_FLOAT_EQ(p1.y(), p2.at(1)); 247 | } 248 | } 249 | } 250 | 251 | TEST(MergingPipeline, setEmptyTransforms) 252 | { 253 | constexpr size_t size = 2; 254 | std::vector maps(size); 255 | std::vector transforms(size); 256 | combine_grids::MergingPipeline merger; 257 | merger.feed(maps.begin(), maps.end()); 258 | merger.setTransforms(transforms.begin(), transforms.end()); 259 | EXPECT_EQ(merger.composeGrids(), nullptr); 260 | EXPECT_EQ(merger.getTransforms().size(), size); 261 | } 262 | 263 | /* empty image may end with identity transform. */ 264 | TEST(MergingPipeline, emptyImageWithTransform) 265 | { 266 | constexpr size_t size = 1; 267 | std::vector maps(size); 268 | std::vector transforms(size); 269 | transforms[0].rotation.z = 1; // set transform to identity 270 | combine_grids::MergingPipeline merger; 271 | merger.feed(maps.begin(), maps.end()); 272 | merger.setTransforms(transforms.begin(), transforms.end()); 273 | EXPECT_EQ(merger.composeGrids(), nullptr); 274 | EXPECT_EQ(merger.getTransforms().size(), size); 275 | } 276 | 277 | /* one image may be empty */ 278 | TEST(MergingPipeline, oneEmptyImage) 279 | { 280 | std::vector maps{nullptr, 281 | loadMap(gmapping_maps[0])}; 282 | combine_grids::MergingPipeline merger; 283 | merger.feed(maps.begin(), maps.end()); 284 | merger.estimateTransforms(); 285 | auto merged_grid = merger.composeGrids(); 286 | auto transforms = merger.getTransforms(); 287 | 288 | EXPECT_VALID_GRID(merged_grid); 289 | // don't use EXPECT_EQ, since it prints too much info 290 | EXPECT_TRUE(maps_equal(*merged_grid, *maps[1])); 291 | // transforms 292 | EXPECT_EQ(transforms.size(), (long unsigned int) 2); 293 | EXPECT_TRUE(isIdentity(transforms[1])); 294 | } 295 | 296 | // non-identity known positions etc. 297 | TEST(MergingPipeline, knownInitPositions) 298 | { 299 | auto maps = loadMaps(gmapping_maps.begin(), gmapping_maps.end()); 300 | combine_grids::MergingPipeline merger; 301 | merger.feed(maps.begin(), maps.end()); 302 | 303 | for (size_t i = 0; i < 5; ++i) { 304 | std::vector transforms{randomTransform(), 305 | randomTransform()}; 306 | merger.setTransforms(transforms.begin(), transforms.end()); 307 | auto merged_grid = merger.composeGrids(); 308 | 309 | EXPECT_VALID_GRID(merged_grid); 310 | } 311 | } 312 | 313 | int main(int argc, char** argv) 314 | { 315 | // ros::Time::init(); 316 | // if (verbose_tests && 317 | // ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, 318 | // ros::console::levels::Debug)) { 319 | // ros::console::notifyLoggerLevelsChanged(); 320 | // } 321 | testing::InitGoogleTest(&argc, argv); 322 | return RUN_ALL_TESTS(); 323 | } 324 | -------------------------------------------------------------------------------- /map_merge/test/testing_helpers.h: -------------------------------------------------------------------------------- 1 | #ifndef TESTING_HELPERS_H_ 2 | #define TESTING_HELPERS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | const float resolution = 0.05f; 16 | 17 | nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename); 18 | void saveMap(const std::string& filename, 19 | const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map); 20 | std::tuple randomAngleTxTy(); 21 | geometry_msgs::msg::Transform randomTransform(); 22 | cv::Mat randomTransformMatrix(); 23 | 24 | /* map_server is really bad. until there is no replacement I will implement it 25 | * by myself */ 26 | template 27 | std::vector loadMaps(InputIt filenames_begin, 28 | InputIt filenames_end) 29 | { 30 | std::vector result; 31 | 32 | for (InputIt it = filenames_begin; it != filenames_end; ++it) { 33 | result.emplace_back(loadMap(*it)); 34 | } 35 | return result; 36 | } 37 | 38 | nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename) 39 | { 40 | cv::Mat lookUpTable(1, 256, CV_8S); 41 | signed char* p = lookUpTable.ptr(); 42 | p[254] = 0; 43 | p[205] = -1; 44 | p[0] = 100; 45 | 46 | cv::Mat img = cv::imread(filename, cv::IMREAD_GRAYSCALE); 47 | if (img.empty()) { 48 | throw std::runtime_error("could not load map"); 49 | } 50 | nav_msgs::msg::OccupancyGrid::SharedPtr grid{new nav_msgs::msg::OccupancyGrid()}; 51 | grid->info.width = static_cast(img.size().width); 52 | grid->info.height = static_cast(img.size().height); 53 | grid->info.resolution = resolution; 54 | grid->data.resize(static_cast(img.size().area())); 55 | cv::Mat grid_view(img.size(), CV_8S, 56 | const_cast(grid->data.data())); 57 | cv::LUT(img, lookUpTable, grid_view); 58 | 59 | return grid; 60 | } 61 | 62 | void saveMap(const std::string& filename, 63 | const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map) 64 | { 65 | cv::Mat lookUpTable(1, 256, CV_8U); 66 | uchar* p = lookUpTable.ptr(); 67 | for (int i = 0; i < 255; ++i) { 68 | if (i >= 0 && i < 10) 69 | p[i] = 254; 70 | else 71 | p[i] = 0; 72 | } 73 | p[255] = 205; 74 | 75 | cv::Mat img(map->info.height, map->info.width, CV_8S, 76 | const_cast(map->data.data())); 77 | cv::Mat out_img; 78 | cv::LUT(img, lookUpTable, out_img); 79 | cv::imwrite(filename, out_img); 80 | } 81 | 82 | std::tuple randomAngleTxTy() 83 | { 84 | static std::mt19937_64 g(156468754 /*magic*/); 85 | std::uniform_real_distribution rotation_dis(0., 2 * std::acos(-1)); 86 | std::uniform_real_distribution translation_dis(-1000, 1000); 87 | 88 | return std::tuple(rotation_dis(g), translation_dis(g), 89 | translation_dis(g)); 90 | } 91 | 92 | geometry_msgs::msg::Transform randomTransform() 93 | { 94 | double angle, tx, ty; 95 | std::tie(angle, tx, ty) = randomAngleTxTy(); 96 | tf2::Transform transform; 97 | tf2::Quaternion rotation; 98 | rotation.setEuler(0., 0., angle); 99 | rotation.normalize(); 100 | transform.setRotation(rotation); 101 | transform.setOrigin(tf2::Vector3(tx, ty, 0.)); 102 | 103 | auto msg = toMsg(transform); 104 | // normalize quaternion such that w > 0 (q and -q represents the same 105 | // transformation) 106 | if (msg.rotation.w < 0.) { 107 | msg.rotation.x *= -1.; 108 | msg.rotation.y *= -1.; 109 | msg.rotation.z *= -1.; 110 | msg.rotation.w *= -1.; 111 | } 112 | 113 | return msg; 114 | } 115 | 116 | cv::Mat randomTransformMatrix() 117 | { 118 | double angle, tx, ty; 119 | std::tie(angle, tx, ty) = randomAngleTxTy(); 120 | cv::Mat transform = 121 | (cv::Mat_(3, 3) << std::cos(angle), -std::sin(angle), tx, 122 | std::sin(angle), std::cos(angle), ty, 0., 0., 1.); 123 | 124 | return transform; 125 | } 126 | 127 | static inline bool isIdentity(const geometry_msgs::msg::Transform& transform) 128 | { 129 | tf2::Transform t; 130 | tf2::fromMsg(transform, t); 131 | return tf2::Transform::getIdentity() == t; 132 | } 133 | 134 | static inline bool isIdentity(const geometry_msgs::msg::Quaternion& rotation) 135 | { 136 | tf2::Quaternion q; 137 | tf2::fromMsg(rotation, q); 138 | return tf2::Quaternion::getIdentity() == q; 139 | } 140 | 141 | // data size is consistent with height and width 142 | static inline bool consistentData(const nav_msgs::msg::OccupancyGrid& grid) 143 | { 144 | return grid.info.width * grid.info.height == grid.data.size(); 145 | } 146 | 147 | // ignores header, map_load_time and origin 148 | // static inline bool operator==(const nav_msgs::msg::OccupancyGrid::SharedPtr grid1, 149 | // const nav_msgs::msg::OccupancyGrid::SharedPtr grid2) 150 | // { 151 | // bool equal = true; 152 | // equal &= grid1->info.width == grid2->info.width; 153 | // equal &= grid1->info.height == grid2->info.height; 154 | // equal &= std::abs(grid1->info.resolution - grid2->info.resolution) < 155 | // std::numeric_limits::epsilon(); 156 | // equal &= grid1->data.size() == grid2->data.size(); 157 | // for (size_t i = 0; i < grid1->data.size(); ++i) { 158 | // equal &= grid1->data[i] == grid2->data[i]; 159 | // } 160 | // return equal; 161 | // } 162 | 163 | // ignores header, map_load_time and origin 164 | static inline bool maps_equal(const nav_msgs::msg::OccupancyGrid& grid1, 165 | const nav_msgs::msg::OccupancyGrid& grid2) 166 | { 167 | // std::cout << "asdasdadsdth: " << std::endl; 168 | bool equal = true; 169 | equal &= grid1.info.width == grid2.info.width; 170 | equal &= grid1.info.height == grid2.info.height; 171 | equal &= std::abs(grid1.info.resolution - grid2.info.resolution) < 172 | std::numeric_limits::epsilon(); 173 | equal &= grid1.data.size() == grid2.data.size(); 174 | for (size_t i = 0; i < grid1.data.size(); ++i) { 175 | equal &= grid1.data[i] == grid2.data[i]; 176 | } 177 | return equal; 178 | } 179 | 180 | #endif // TESTING_HELPERS_H_ 181 | --------------------------------------------------------------------------------