PORT: multirobot_map_merge (#8)

* Baby steps * Compile without implementations * Compile combined_grids * Compile part of principal node map_merge.cpp * Finish 90% of main node * Move ros::names implementation to own file * Replace ros1 while loops threads with ros2 timers * Fix checking of map type for adding robots. bypass mutex for now * Working version without init poses, still verbose logs * Add namespace to explore_lite Add tb3 launch files for multirobot tests Finish multirobot map merge with poses known * Add files for tb3 demo * Fix launch file for demo with robot poses * Fix comments on locking * Change deprecated Ptr to SharedPtr in ros2 msgs variables * Change ConstPtr to ConstSharedPtr for ROS2 * Fix minor warnings in explor_lite * Attempt fix slam toolbox * Add launch and remove old ones * Add copyright * Add readme with instruction on how to run the demo in sim * Fix readme and port from_map_server launch file * Fix QoS for suscriptions with TRansientlocal
2021-12-26 20:28:18 -05:00 · 2021-12-26 20:28:18 -05:00 · f808bc404a
parent 8005841a07
commit f808bc404a
35 changed files with 4363 additions and 20 deletions
--- a/README.md
+++ b/README.md
@ -1,7 +1,8 @@
 # m-explore ROS2 port

-ROS2 package port for (not yet multi) robot exploration of [m-explore](https://github.com/hrnr/m-explore). Currently tested on Eloquent, Dashing, Foxy, and Galactic distros.
+ROS2 package port for multi robot exploration of [m-explore](https://github.com/hrnr/m-explore). Currently tested on Eloquent, Dashing, Foxy, and Galactic distros.

+## Autonomous exploration

 ### TB3
 https://user-images.githubusercontent.com/8033598/128805356-be90a880-16c6-4fc9-8f54-e3302873dc8c.mp4
@ -11,7 +12,6 @@ https://user-images.githubusercontent.com/8033598/128805356-be90a880-16c6-4fc9-8
 https://user-images.githubusercontent.com/18732666/128493567-6841dde0-2250-4d81-9bcb-8b216e0fb34d.mp4


-
 Installing
 ----------

@ -32,8 +32,12 @@ ros2 run explore_lite explore --ros-args --params-file <path_to_ros_ws>/m-explor

 ### Running the demo with TB3
 Install nav2 and tb3 simulation. You can follow the [tutorial](https://navigation.ros.org/getting_started/index.html#installation).
+
 Then just run the nav2 stack with slam:
+
 ```
+export TURTLEBOT3_MODEL=waffle
+export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models
 ros2 launch nav2_bringup tb3_simulation_launch.py slam:=true
 ```

@ -42,8 +46,9 @@ And run this package with
 ros2 launch explore_lite explore.launch.py
 ```

+You can open an rviz2 and add the exploration frontiers marker to see the algorithm working and choose a frontier to explore.

-#### TB3 troubleshooting
+#### TB3 troubleshooting (with foxy)
 If you have trouble with TB3 in simulation like we did, add this extra steps for configuring it.

 ```
@ -53,13 +58,59 @@ sudo rm -rf /opt/ros/${ROS_DISTRO}/share/turtlebot3_simulations
 sudo git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations /opt/ros/${ROS_DISTRO}/share/turtlebot3_simulations
 export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_simulations/turtlebot3_gazebo/models
 ```
+
 Then you'll be able to run it.

+## Multirobot map merge
+
+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:
+
+### Known initial poses (best results)
+https://user-images.githubusercontent.com/8033598/144522712-c31fb4bb-bb5a-4859-b3e1-8ad665f80696.mp4
+
+### Unknown initial poses 
+It works better if the robots start very close (< 3 meters) to each other so their relative positions can be calculated properly.
+
+https://user-images.githubusercontent.com/8033598/144522696-517d54fd-74d0-4c55-9aca-f1b9679afb3e.mp4
+
+### ROS2 requirements
+
+#### SLAM
+Because of the logic that merges the maps, currently as a straight forward 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 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 [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.
+
+
+```
+cd <your/ros2_ws/src>
+git clone https://github.com/charlielito/slam_gmapping.git --branch feature/namespace_launch
+cd ..
+colcon build --symlink-install --packages-up-to slam_gmapping
+```
+#### Nav2 config files
+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 branch and since some breaking changes were introduced in this branch, 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).
+
+### Running the demo with TB3
+First you'll need to launch the whole simulation stack, nav2 stacks and slam stacks per robot. For that just launch::
+```
+export TURTLEBOT3_MODEL=waffle
+export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models
+ros2 launch multirobot_map_merge multi_tb3_simulation_launch.py slam_gmapping:=True
+```
+Now run the merging node:
+```
+ros2 launch multirobot_map_merge map_merge.launch.py
+```
+
+By default the demo runs with known initial poses. You can change that by launching again both launch commands with with the flag `known_init_poses:=False`
+
+Then you can start moving each robot with its corresponding rviz2 interface sending nav2 goals. To see the map merged just launch rviz2:
+```
+rviz2 -d <your/ros2_ws>/src/m-explore-ros2/map_merge/launch/map_merge.rviz
+```
+
 WIKI
 ----
 No wiki yet.

-
 COPYRIGHT
 ---------

--- a/explore/config/params.yaml
+++ b/explore/config/params.yaml
@ -1,4 +1,4 @@
-explore_node:
+/**:
  ros__parameters:
    robot_base_frame: base_link
    costmap_topic: map
--- a/explore/include/explore/explore.h
+++ b/explore/include/explore/explore.h
@ -59,11 +59,11 @@

 using namespace std::placeholders;
 #ifdef ELOQUENT
-#define ACTION_NAME "/NavigateToPose"
+#define ACTION_NAME "NavigateToPose"
 #elif DASHING
-#define ACTION_NAME "/NavigateToPose"
+#define ACTION_NAME "NavigateToPose"
 #else
-#define ACTION_NAME "/navigate_to_pose"
+#define ACTION_NAME "navigate_to_pose"
 #endif
 namespace explore
 {
--- a/explore/launch/explore.launch.py
+++ b/explore/launch/explore.launch.py
@ -14,18 +14,33 @@ def generate_launch_description():
        get_package_share_directory("explore_lite"), "config", "params.yaml"
    )
    use_sim_time = LaunchConfiguration("use_sim_time")
+    namespace = LaunchConfiguration("namespace")

    declare_use_sim_time_argument = DeclareLaunchArgument(
        "use_sim_time", default_value="true", description="Use simulation/Gazebo clock"
    )
+    declare_namespace_argument = DeclareLaunchArgument(
+        "namespace",
+        default_value="",
+        description="Namespace for the explore node",
+    )
+
+    # Map fully qualified names to relative ones so the node's namespace can be prepended.
+    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
+    # https://github.com/ros/geometry2/issues/32
+    # https://github.com/ros/robot_state_publisher/pull/30
+    remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]

    node = Node(
        package="explore_lite",
        name="explore_node",
+        namespace=namespace,
        executable="explore",
        parameters=[config, {"use_sim_time": use_sim_time}],
        output="screen",
+        remappings=remappings,
    )
    ld.add_action(declare_use_sim_time_argument)
+    ld.add_action(declare_namespace_argument)
    ld.add_action(node)
    return ld
--- a/explore/src/costmap_client.cpp
+++ b/explore/src/costmap_client.cpp
@ -158,9 +158,7 @@ void Costmap2DClient::updateFullMap(

  // lock as we are accessing raw underlying map
  auto* mutex = costmap_.getMutex();
-
-  // TODO: fix compilation error in following line
-  // std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
+  std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*mutex);

  // fill map with data
  unsigned char* costmap_data = costmap_.getCharMap();
@ -194,9 +192,7 @@ void Costmap2DClient::updatePartialMap(

  // lock as we are accessing raw underlying map
  auto* mutex = costmap_.getMutex();
-
-  // TODO: fix compilation error in following line
-  // std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
+  std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*mutex);

  size_t costmap_xn = costmap_.getSizeInCellsX();
  size_t costmap_yn = costmap_.getSizeInCellsY();
@ -226,6 +222,7 @@ void Costmap2DClient::updatePartialMap(
 geometry_msgs::msg::Pose Costmap2DClient::getRobotPose() const
 {
  geometry_msgs::msg::PoseStamped robot_pose;
+  geometry_msgs::msg::Pose empty_pose;
  robot_pose.header.frame_id = robot_base_frame_;
  robot_pose.header.stamp = node_.now();

@ -240,21 +237,21 @@ geometry_msgs::msg::Pose Costmap2DClient::getRobotPose() const
                          "No Transform available Error looking up robot pose: "
                          "%s\n",
                          ex.what());
-    return {};
+    return empty_pose;
  } catch (tf2::ConnectivityException& ex) {
    RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000,
                          "Connectivity Error looking up robot pose: %s\n",
                          ex.what());
-    return {};
+    return empty_pose;
  } catch (tf2::ExtrapolationException& ex) {
    RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000,
                          "Extrapolation Error looking up robot pose: %s\n",
                          ex.what());
-    return {};
+    return empty_pose;
  } catch (tf2::TransformException& ex) {
    RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, "Other error: %s\n",
                          ex.what());
-    return {};
+    return empty_pose;
  }

  return robot_pose.pose;
--- a/explore/src/explore.cpp
+++ b/explore/src/explore.cpp
@ -131,8 +131,14 @@ void Explore::visualizeFrontiers(
  m.color.b = 255;
  m.color.a = 255;
  // lives forever
-  // m.lifetime = ros::Duration(0);
-  m.lifetime = rclcpp::Duration(0);
+#ifdef ELOQUENT
+  m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning
+#elif DASHING
+  m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning
+#else
+  m.lifetime = rclcpp::Duration::from_seconds(0); // foxy onwards
+#endif
+  // m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in galactic
  m.frame_locked = true;

  // weighted frontiers are always sorted
--- a/map_merge/CHANGELOG.rst
+++ b/map_merge/CHANGELOG.rst
@ -0,0 +1,58 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package multirobot_map_merge
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.1.4 (2021-01-07)
+------------------
+* use C++14
+* support both OpenCV 3 and OpenCV 4 (support Debian Buster)
+* Contributors: Jiri Horner
+
+2.1.3 (2021-01-03)
+------------------
+* add missing dependencies to catkin_package calls
+* update map_merge for OpenCV 4
+* Contributors: Jiri Horner
+
+2.1.2 (2021-01-02)
+------------------
+* support for ROS Melodic
+* bugfix: zero resolution of the merged grid for known initial posiiton
+* bugfix: estimation_confidence parameter had no effect
+* map_merge: set origin of merged grid in its centre
+* map_merge: add new launch file
+  * map_merge with 2 maps served by map_server
+* bugfix: ensure that we never output map with 0 resolution
+* bugfix: nullptr derefence while setting resolution of output grid
+* Contributors: Jiri Horner
+
+2.1.1 (2017-12-16)
+------------------
+* fix bugs in CMakeLists.txt: install nodes in packages, so they get shipped in debian packages. fixes `#11 <https://github.com/hrnr/m-explore/issues/11>`_
+* map_merge: add bibtex to wiki page
+* Contributors: Jiri Horner
+
+2.1.0 (2017-10-30)
+------------------
+* no major changes. Released together with explore_lite.
+
+2.0.0 (2017-03-26)
+------------------
+* map_merge: upgrade to package format 2
+* node completely rewritten based on my work included in opencv
+* uses more reliable features by default -> more robust merging
+* known_init_poses is now by default false to make it easy to start for new users
+* Contributors: Jiri Horner
+
+1.0.1 (2017-03-25)
+------------------
+* map_merge: use inverted tranform
+  * transform needs to be inverted before using
+* map_merge: change package description
+  * we support merging with unknown initial positions
+* Contributors: Jiri Horner
+
+1.0.0 (2016-05-11)
+------------------
+* initial release
+* Contributors: Jiri Horner
--- a/map_merge/CMakeLists.txt
+++ b/map_merge/CMakeLists.txt
@ -0,0 +1,144 @@
+cmake_minimum_required(VERSION 3.5)
+project(multirobot_map_merge)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(image_geometry REQUIRED)
+find_package(map_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+find_package(Boost REQUIRED COMPONENTS thread)
+
+# OpenCV is required for merging without initial positions
+find_package(OpenCV REQUIRED)
+if("${OpenCV_VERSION}" VERSION_LESS "3.0")
+message(FATAL_ERROR "This package needs OpenCV >= 3.0")
+endif()
+if("${OpenCV_VERSION}" VERSION_LESS "4.0")
+message(WARNING "This package supports OpenCV 3, but some features may not be "
+"available. Upgrade to OpenCV 4 to take advantage of all features.")
+endif()
+
+set(DEPENDENCIES
+  rclcpp
+  geometry_msgs
+  image_geometry
+  map_msgs
+  nav_msgs
+  tf2_geometry_msgs
+  OpenCV
+)
+
+## Specify additional locations of header files
+include_directories(
+  # ${Boost_INCLUDE_DIRS}
+  # ${OpenCV_INCLUDE_DIRS}
+  include
+)
+
+install(
+  DIRECTORY include/map_merge/
+  DESTINATION include/map_merge/
+)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# we want static linking for now
+add_library(combine_grids STATIC
+src/combine_grids/grid_compositor.cpp
+src/combine_grids/grid_warper.cpp
+src/combine_grids/merging_pipeline.cpp
+)
+
+add_executable(map_merge
+  src/map_merge.cpp
+)
+
+#############
+## Install ##
+#############
+target_include_directories(map_merge PUBLIC
+$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+$<INSTALL_INTERFACE:include>)
+target_include_directories(combine_grids PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include>"
+  ${rclcpp_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+target_link_libraries(combine_grids ${rclcpp_LIBRARIES} ${OpenCV_LIBS})
+# target_link_libraries(combine_grids ${OpenCV_LIBRARIES})
+
+target_link_libraries(map_merge combine_grids)
+# target_link_libraries(map_merge)
+
+ament_target_dependencies(map_merge ${DEPENDENCIES})
+
+install(
+  TARGETS combine_grids
+  EXPORT export_combine_grids
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+install(TARGETS map_merge
+  DESTINATION lib/${PROJECT_NAME})
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+
+ament_export_include_directories(include)
+ament_export_libraries(combine_grids)
+ament_package()
+
+
+#############
+## Testing ##
+#############
+# if(CATKIN_ENABLE_TESTING)
+#   find_package(roslaunch REQUIRED)
+
+#   # download test data
+#   set(base_url https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge)
+#   catkin_download_test_data(${PROJECT_NAME}_map00.pgm ${base_url}/hector_maps/map00.pgm MD5 915609a85793ec1375f310d44f2daf87)
+#   catkin_download_test_data(${PROJECT_NAME}_map05.pgm ${base_url}/hector_maps/map05.pgm MD5 cb9154c9fa3d97e5e992592daca9853a)
+#   catkin_download_test_data(${PROJECT_NAME}_2011-08-09-12-22-52.pgm ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm MD5 3c2c38e7dec2b7a67f41069ab58badaa)
+#   catkin_download_test_data(${PROJECT_NAME}_2012-01-28-11-12-01.pgm ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm MD5 681e704044889c95e47b0c3aadd81f1e)
+
+#   catkin_add_gtest(test_merging_pipeline test/test_merging_pipeline.cpp)
+#   # ensure that test data are downloaded before we run tests
+#   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)
+#   target_link_libraries(test_merging_pipeline combine_grids ${catkin_LIBRARIES})
+
+#   # test all launch files
+#   # do not test from_map_server.launch as we don't want to add dependency on map_server and this
+#   # launchfile is not critical
+#   roslaunch_add_file_check(launch/map_merge.launch)
+#   roslaunch_add_file_check(launch/experiments)
+# endif()
+
+
+
--- a/map_merge/config/params.yaml
+++ b/map_merge/config/params.yaml
@ -0,0 +1,24 @@
+map_merge:
+  ros__parameters:
+    merging_rate: 4.0
+    discovery_rate: 0.05
+    estimation_rate: 0.5
+    estimation_confidence: 0.6
+    robot_map_topic: map
+    robot_map_updates_topic: map_udpates
+    robot_namespace: ""
+    merged_map_topic: map
+    world_frame: world
+    known_init_poses: true
+    # known_init_poses: false
+
+    # Define here robots' positions in the map if known_init_poses is true
+    /robot1/map_merge/init_pose_x: 0.0
+    /robot1/map_merge/init_pose_y: 0.0
+    /robot1/map_merge/init_pose_z: 0.0
+    /robot1/map_merge/init_pose_yaw: 0.0
+
+    /robot2/map_merge/init_pose_x: -3.0
+    /robot2/map_merge/init_pose_y: 1.0
+    /robot2/map_merge/init_pose_z: 0.0
+    /robot2/map_merge/init_pose_yaw: 0.0
--- a/map_merge/doc/architecture.dia
+++ b/map_merge/doc/architecture.dia
--- a/map_merge/doc/screenshot.jpg
+++ b/map_merge/doc/screenshot.jpg
--- a/map_merge/doc/wiki_doc.txt
+++ b/map_merge/doc/wiki_doc.txt
@ -0,0 +1,168 @@
+<<PackageHeader(multirobot_map_merge)>>
+
+<<GitHubIssues(hrnr/m-explore)>>
+
+<<TOC(4)>>
+
+== Overview ==
+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.
+
+{{attachment:screenshot.jpg||width="755px"}}
+
+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.
+
+<<Youtube(8Adrn29BVbM&rel=0)>>
+
+{{{multirobot_map_merge}}} does not depend on any particular communication between robots.
+
+== Architecture ==
+
+{{{multirobot_map_merge}}} finds robot maps dynamically and new robots can be added to system at any time.
+
+{{attachment:architecture.svg||width="755px"}}
+
+To make this dynamic behaviour possible there are some constrains placed on robots. First all robots must publish map under `<robot_namespace>/map`, where topic name (`map`) is configurable, but must be same for all robots. For each robot `<robot_namespace>` will be of cause different.
+
+This node support merging maps with known initial positions of the robots or without. See below for details.
+
+== Merging modes ==
+
+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.
+
+=== merging with known initial positions ===
+
+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 `<robot_namespace>/map_merge/init_pose` parameters. These positions should be in `world_frame`. See [[#ROS API]].
+
+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).
+
+=== merging without known initial positions ===
+
+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.
+
+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.
+
+== ROS API ==
+{{{
+#!clearsilver CS/NodeAPI
+
+name = map_merge
+desc = Provides map merging services offered by this package. Dynamically looks for new robots in the system and merges their maps.
+
+pub {
+  0.name  = map
+  0.type = nav_msgs/OccupancyGrid
+  0.desc = Merged map from all robots in the system.
+}
+sub {
+  0.name = <robot_namespace>/map
+  0.type = nav_msgs/OccupancyGrid
+  0.desc = Local map for specific robot.
+
+  1.name = <robot_namespace>/map_updates
+  1.type = map_msgs/OccupancyGridUpdate
+  1.desc = Local map updates for specific robot. Most of the <<MsgLink(nav_msgs/OccupancyGrid)>> 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.
+}
+
+param {
+  group.0 {
+    name = ROBOT PARAMETERS
+    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]]
+
+    0.name = <robot_namespace>/map_merge/init_pose_x
+    0.default = `<no_default>`
+    0.type = double
+    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.
+
+    1.name = <robot_namespace>/map_merge/init_pose_y
+    1.default = `<no_default>`
+    1.type = double
+    1.desc = `y` coordinate of robot initial position in `world_frame`.
+
+    2.name = <robot_namespace>/map_merge/init_pose_z
+    2.default = `<no_default>`
+    2.type = double
+    2.desc = `z` coordinate of robot initial position in `world_frame`.
+
+    3.name = <robot_namespace>/map_merge/init_pose_yaw
+    3.default = `<no_default>`
+    3.type = double
+    3.desc = `yaw` component of robot initial position in `world_frame`. Represents robot rotation in radians.
+  }
+
+  group.1 {
+    name = NODE PARAMETERS
+    desc = Parameters that should be defined in the namespace of this node.
+
+    0.name = ~robot_map_topic
+    0.default = `map`
+    0.type = string
+    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).
+
+    1.name = ~robot_map_updates_topic
+    1.default = `map_updates`
+    1.type = string
+    1.desc = Name of robot map updates topic of <<MsgLink(map_msgs/OccupancyGridUpdate)>> 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.
+
+    2.name = ~robot_namespace
+    2.default =  `<empty string>`
+    2.type = string
+    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).
+
+    3.name = ~known_init_poses
+    3.default = `true`
+    3.type = bool
+    3.desc = Selects between merging modes. `true` if merging with known initial positions. See [[#Merging modes]]
+
+    4.name = ~merged_map_topic
+    4.default = `map`
+    4.type = string
+    4.desc = Topic name where merged map will be published.
+
+    5.name = ~world_frame
+    5.default = `world`
+    5.type = string
+    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.
+
+    6.name = ~merging_rate
+    6.default = `4.0`
+    6.type = double
+    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.
+
+    7.name = ~discovery_rate
+    7.default = `0.05`
+    7.type = double
+    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.
+
+    8.name = ~estimation_rate
+    8.default = `0.5`
+    8.type = double
+    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.
+
+    9.name = ~estimation_confidence
+    9.default = `1.0`
+    9.type = double
+    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].
+  }
+}
+}}}
+
+== Acknowledgements ==
+
+This package was developed as part of my bachelor thesis at [[http://www.mff.cuni.cz/to.en/|Charles University]] in Prague.
+
+{{{
+@masterthesis{Hörner2016,
+  author = {Jiří Hörner},
+  title = {Map-merging for multi-robot system},
+  address = {Prague},
+  year = {2016},
+  school = {Charles University in Prague, Faculty of Mathematics and Physics},
+  type = {Bachelor's thesis},
+  URL = {https://is.cuni.cz/webapps/zzp/detail/174125/},
+}
+}}}
+
+Idea for dynamic robot discovery is from [[map_merging]] package from Zhi Yan. Merging algorithm and configuration are different.
+
+## AUTOGENERATED DON'T DELETE
+## CategoryPackage
--- a/map_merge/include/combine_grids/grid_compositor.h
+++ b/map_merge/include/combine_grids/grid_compositor.h
@ -0,0 +1,60 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#ifndef GRID_COMPOSITOR_H_
+#define GRID_COMPOSITOR_H_
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <opencv2/core/utility.hpp>
+
+namespace combine_grids
+{
+namespace internal
+{
+class GridCompositor
+{
+public:
+  nav_msgs::msg::OccupancyGrid::SharedPtr compose(const std::vector<cv::Mat>& grids,
+                                       const std::vector<cv::Rect>& rois);
+};
+
+}  // namespace internal
+
+}  // namespace combine_grids
+
+#endif  // GRID_COMPOSITOR_H_
--- a/map_merge/include/combine_grids/grid_warper.h
+++ b/map_merge/include/combine_grids/grid_warper.h
@ -0,0 +1,61 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#ifndef GRID_WARPER_H_
+#define GRID_WARPER_H_
+
+#include <opencv2/core/utility.hpp>
+
+namespace combine_grids
+{
+namespace internal
+{
+class GridWarper
+{
+public:
+  cv::Rect warp(const cv::Mat& grid, const cv::Mat& transform,
+                cv::Mat& warped_grid);
+
+private:
+  cv::Rect warpRoi(const cv::Mat& grid, const cv::Mat& transform);
+};
+
+}  // namespace internal
+
+}  // namespace combine_grids
+
+#endif  // GRID_WARPER_H_
--- a/map_merge/include/combine_grids/merging_pipeline.h
+++ b/map_merge/include/combine_grids/merging_pipeline.h
@ -0,0 +1,146 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez. 
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#ifndef MERGING_PIPELINE_H_
+#define MERGING_PIPELINE_H_
+
+#include <vector>
+
+#include <geometry_msgs/msg/transform.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <opencv2/core/utility.hpp>
+
+namespace combine_grids
+{
+enum class FeatureType { AKAZE, ORB, SURF };
+
+/**
+ * @brief Pipeline for merging overlapping occupancy grids
+ * @details Pipeline works on internally stored grids.
+ */
+class MergingPipeline
+{
+public:
+  template <typename InputIt>
+  void feed(InputIt grids_begin, InputIt grids_end);
+  bool estimateTransforms(FeatureType feature = FeatureType::AKAZE,
+                          double confidence = 1.0);
+  nav_msgs::msg::OccupancyGrid::SharedPtr composeGrids();
+
+  std::vector<geometry_msgs::msg::Transform> getTransforms() const;
+  template <typename InputIt>
+  bool setTransforms(InputIt transforms_begin, InputIt transforms_end);
+
+private:
+  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> grids_;
+  std::vector<cv::Mat> images_;
+  std::vector<cv::Mat> transforms_;
+  double max_confidence_achieved_ = 0.0;
+};
+
+template <typename InputIt>
+void MergingPipeline::feed(InputIt grids_begin, InputIt grids_end)
+{
+  static_assert(std::is_assignable<nav_msgs::msg::OccupancyGrid::ConstSharedPtr&,
+                                   decltype(*grids_begin)>::value,
+                "grids_begin must point to nav_msgs::msg::OccupancyGrid::ConstSharedPtr "
+                "data");
+
+  // we can't reserve anything, because we want to support just InputIt and
+  // their guarantee validity for only single-pass algos
+  images_.clear();
+  grids_.clear();
+  for (InputIt it = grids_begin; it != grids_end; ++it) {
+    if (*it && !(*it)->data.empty()) {
+      grids_.push_back(*it);
+      /* convert to opencv images. it creates only a view for opencv and does
+       * not copy or own actual data. */
+      images_.emplace_back((*it)->info.height, (*it)->info.width, CV_8UC1,
+                           const_cast<signed char*>((*it)->data.data()));
+    } else {
+      grids_.emplace_back();
+      images_.emplace_back();
+    }
+  }
+}
+
+template <typename InputIt>
+bool MergingPipeline::setTransforms(InputIt transforms_begin,
+                                    InputIt transforms_end)
+{
+  static_assert(std::is_assignable<geometry_msgs::msg::Transform&,
+                                   decltype(*transforms_begin)>::value,
+                "transforms_begin must point to geometry_msgs::msg::Transform "
+                "data");
+
+  decltype(transforms_) transforms_buf;
+  for (InputIt it = transforms_begin; it != transforms_end; ++it) {
+    const geometry_msgs::msg::Quaternion& q = it->rotation;
+    if ((q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) <
+        std::numeric_limits<double>::epsilon()) {
+      // represents invalid transform
+      transforms_buf.emplace_back();
+      continue;
+    }
+    double s = 2.0 / (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
+    double a = 1 - q.y * q.y * s - q.z * q.z * s;
+    double b = q.x * q.y * s + q.z * q.w * s;
+    double tx = it->translation.x;
+    double ty = it->translation.y;
+    cv::Mat transform = cv::Mat::eye(3, 3, CV_64F);
+    transform.at<double>(0, 0) = transform.at<double>(1, 1) = a;
+    transform.at<double>(1, 0) = b;
+    transform.at<double>(0, 1) = -b;
+    transform.at<double>(0, 2) = tx;
+    transform.at<double>(1, 2) = ty;
+
+    transforms_buf.emplace_back(std::move(transform));
+  }
+
+  if (transforms_buf.size() != images_.size()) {
+    return false;
+  }
+  std::swap(transforms_, transforms_buf);
+
+  return true;
+}
+
+}  // namespace combine_grids
+
+#endif  // MERGING_PIPELINE_H_
--- a/map_merge/include/map_merge/map_merge.h
+++ b/map_merge/include/map_merge/map_merge.h
@ -0,0 +1,128 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, Zhi Yan.
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#ifndef MAP_MERGE_H_
+#define MAP_MERGE_H_
+
+#include <atomic>
+#include <forward_list>
+#include <mutex>
+#include <unordered_map>
+
+#include <combine_grids/merging_pipeline.h>
+#include <geometry_msgs/msg/transform.hpp>
+#include <map_msgs/msg/occupancy_grid_update.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <boost/thread.hpp>
+
+namespace map_merge
+{
+struct MapSubscription {
+  // protects consistency of writable_map and readonly_map
+  // also protects reads and writes of shared_ptrs
+  std::mutex mutex;
+
+  geometry_msgs::msg::Transform initial_pose;
+  nav_msgs::msg::OccupancyGrid::SharedPtr writable_map;
+  nav_msgs::msg::OccupancyGrid::ConstSharedPtr readonly_map;
+
+  // ros::Subscriber map_sub;
+  // ros::Subscriber map_updates_sub;
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub;
+  rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::SharedPtr map_updates_sub;
+};
+
+class MapMerge : public rclcpp::Node
+{
+private:
+  /* parameters */
+  double merging_rate_;
+  double discovery_rate_;
+  double estimation_rate_;
+  double confidence_threshold_;
+  std::string robot_map_topic_;
+  std::string robot_map_updates_topic_;
+  std::string robot_namespace_;
+  std::string world_frame_;
+  bool have_initial_poses_;
+
+  // publishing
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr merged_map_publisher_;
+  // maps robots namespaces to maps. does not own
+  std::unordered_map<std::string, MapSubscription*> robots_;
+  // owns maps -- iterator safe
+  std::forward_list<MapSubscription> subscriptions_;
+  size_t subscriptions_size_;
+  boost::shared_mutex subscriptions_mutex_;
+  combine_grids::MergingPipeline pipeline_;
+  std::mutex pipeline_mutex_;
+
+  rclcpp::Logger logger_ = rclcpp::get_logger("MapMergeNode");
+
+  // timers
+  rclcpp::TimerBase::SharedPtr map_merging_timer_;
+  rclcpp::TimerBase::SharedPtr topic_subscribing_timer_;
+  rclcpp::TimerBase::SharedPtr pose_estimation_timer_;
+
+  std::string robotNameFromTopic(const std::string& topic);
+  // bool isRobotMapTopic(const ros::master::TopicInfo& topic);
+  bool isRobotMapTopic(const std::string topic, std::string type);
+  bool getInitPose(const std::string& name, geometry_msgs::msg::Transform& pose);
+
+  void fullMapUpdate(const nav_msgs::msg::OccupancyGrid::SharedPtr msg,
+                     MapSubscription& map);
+  void partialMapUpdate(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg,
+                        MapSubscription& map);
+
+public:
+  MapMerge();
+
+  void topicSubscribing();
+  void mapMerging();
+  /**
+   * @brief Estimates initial positions of grids
+   * @details Relevant only if initial poses are not known
+   */
+  void poseEstimation();
+};
+
+}  // namespace map_merge
+
+#endif /* MAP_MERGE_H_ */
--- a/map_merge/include/map_merge/ros1_names.hpp
+++ b/map_merge/include/map_merge/ros1_names.hpp
@ -0,0 +1,141 @@
+/*
+* Copyright (C) 2009, Willow Garage, Inc.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*   * Redistributions of source code must retain the above copyright notice,
+*     this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above copyright
+*     notice, this list of conditions and the following disclaimer in the
+*     documentation and/or other materials provided with the distribution.
+*   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived from
+*     this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*/
+#include <ctype.h>
+#include <string>
+#include <sstream>
+
+// Almost all code was taken from http://docs.ros.org/en/indigo/api/roscpp/html/names_8cpp_source.html
+
+class InvalidNameException : public std::runtime_error
+{
+public:
+  InvalidNameException(const std::string& msg)
+  : std::runtime_error(msg)
+  {}
+};
+
+namespace ros1_names
+{
+
+bool isValidCharInName(char c)
+{
+  if (isalnum(c) || c == '/' || c == '_')
+  {
+    return true;
+  }
+
+  return false;
+}
+
+bool validate(const std::string& name, std::string& error)
+{
+  if (name.empty())
+  {
+    return true;
+  }
+
+  // First element is special, can be only ~ / or alpha
+  char c = name[0];
+  if (!isalpha(c) && c != '/' && c != '~')
+  {
+    std::stringstream ss;
+    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 ~.";
+    error = ss.str();
+    return false;
+  }
+
+  for (size_t i = 1; i < name.size(); ++i)
+  {
+    c = name[i];
+    if (!isValidCharInName(c))
+    {
+      std::stringstream ss;
+      ss << "Character [" << c << "] at element [" << i << "] is not valid in Graph Resource Name [" << name <<"].  Valid characters are a-z, A-Z, 0-9, / and _.";
+      error = ss.str();
+
+      return false;
+    }
+  }
+
+  return true;
+}
+
+std::string parentNamespace(const std::string& name)
+{
+  std::string error;
+  if (!validate(name, error))
+  {
+        throw InvalidNameException(error);
+  }
+
+  if (!name.compare(""))  return "";
+  if (!name.compare("/")) return "/"; 
+
+  std::string stripped_name;
+
+  // rstrip trailing slash
+  if (name.find_last_of('/') == name.size()-1)
+    stripped_name = name.substr(0, name.size() -2);
+  else
+    stripped_name = name;
+
+  //pull everything up to the last /
+  size_t last_pos = stripped_name.find_last_of('/');
+  if (last_pos == std::string::npos)
+  {
+    return "";
+  }
+  else if (last_pos == 0)
+    return "/";
+  return stripped_name.substr(0, last_pos);
+}
+
+std::string clean(const std::string& name)
+{
+  std::string clean = name;
+
+  size_t pos = clean.find("//");
+  while (pos != std::string::npos)
+  {
+    clean.erase(pos, 1);
+    pos = clean.find("//", pos);
+  }
+
+  if (*clean.rbegin() == '/')
+  {
+    clean.erase(clean.size() - 1, 1);
+  }
+
+  return clean;
+}
+
+std::string append(const std::string& left, const std::string& right)
+{
+  return clean(left + "/" + right);
+}
+
+} // namespace ros1_names
--- a/map_merge/launch/from_map_server.launch.py
+++ b/map_merge/launch/from_map_server.launch.py
@ -0,0 +1,141 @@
+# showcases map_merge with static maps served by map_server
+
+# you can run this with test maps provided in m-explore-extra repo
+# https://github.com/hrnr/m-explore-extra
+
+# 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
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node, PushRosNamespace
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    EmitEvent,
+    GroupAction,
+    RegisterEventHandler,
+)
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.events import Shutdown
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+    config = os.path.join(
+        get_package_share_directory("multirobot_map_merge"), "config", "params.yaml"
+    )
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    namespace = LaunchConfiguration("namespace")
+    known_init_poses = LaunchConfiguration("known_init_poses")
+    rviz = LaunchConfiguration("rviz")
+
+    declare_use_sim_time_argument = DeclareLaunchArgument(
+        "use_sim_time", default_value="true", description="Use simulation/Gazebo clock"
+    )
+    declare_namespace_argument = DeclareLaunchArgument(
+        "namespace",
+        default_value="/",
+        description="Namespace for the explore node",
+    )
+    declare_known_init_poses_argument = DeclareLaunchArgument(
+        "known_init_poses",
+        default_value="False",
+        description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file",
+    )
+    declare_rviz_argument = DeclareLaunchArgument(
+        "rviz",
+        default_value="False",
+        description="Run rviz2",
+    )
+
+    num_maps = 2
+    group_actions = []
+
+    for i in range(num_maps):
+        map_argument_name = f"map{i+1}"
+        map_yaml_file = LaunchConfiguration(map_argument_name)
+        declare_map_argument = DeclareLaunchArgument(
+            map_argument_name,
+            default_value=f"{map_argument_name}.yaml",
+            description="Full path to map yaml file to load",
+        )
+        map_server = Node(
+            package="nav2_map_server",
+            executable="map_server",
+            name="map_server",
+            namespace=f"robot{i+1}",
+            output="screen",
+            parameters=[
+                {"yaml_filename": map_yaml_file},
+                {"use_sim_time": use_sim_time},
+            ],
+        )
+        map_server_manager = Node(
+            package="nav2_lifecycle_manager",
+            executable="lifecycle_manager",
+            name="lifecycle_manager_localization",
+            output="screen",
+            namespace=f"robot{i+1}",
+            parameters=[
+                {"use_sim_time": use_sim_time},
+                {"autostart": True},
+                {"node_names": ["map_server"]},
+            ],
+        )
+        group_action = GroupAction(
+            [
+                PushRosNamespace(namespace=namespace),
+                map_server_manager,
+                map_server,
+                declare_map_argument,
+            ]
+        )
+
+        group_actions.append(group_action)
+
+    node = Node(
+        package="multirobot_map_merge",
+        name="map_merge",
+        namespace=namespace,
+        executable="map_merge",
+        parameters=[
+            config,
+            {"use_sim_time": use_sim_time},
+            {"known_init_poses": known_init_poses},
+        ],
+        output="screen",
+    )
+
+    rviz_config_file = os.path.join(
+        get_package_share_directory("multirobot_map_merge"), "launch", "map_merge.rviz"
+    )
+    start_rviz_cmd = Node(
+        condition=IfCondition(rviz),
+        package="rviz2",
+        executable="rviz2",
+        arguments=["-d", rviz_config_file],
+        output="screen",
+    )
+    exit_event_handler = RegisterEventHandler(
+        condition=IfCondition(rviz),
+        event_handler=OnProcessExit(
+            target_action=start_rviz_cmd,
+            on_exit=EmitEvent(event=Shutdown(reason="rviz exited")),
+        ),
+    )
+
+    ld.add_action(declare_use_sim_time_argument)
+    ld.add_action(declare_known_init_poses_argument)
+    ld.add_action(declare_namespace_argument)
+    ld.add_action(declare_rviz_argument)
+    for group_action in group_actions:
+        ld.add_action(group_action)
+    ld.add_action(node)
+    ld.add_action(start_rviz_cmd)
+    ld.add_action(exit_event_handler)
+
+    return ld
--- a/map_merge/launch/map_merge.launch.py
+++ b/map_merge/launch/map_merge.launch.py
@ -0,0 +1,57 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+    config = os.path.join(
+        get_package_share_directory("multirobot_map_merge"), "config", "params.yaml"
+    )
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    namespace = LaunchConfiguration("namespace")
+    known_init_poses = LaunchConfiguration("known_init_poses")
+
+    declare_use_sim_time_argument = DeclareLaunchArgument(
+        "use_sim_time", default_value="true", description="Use simulation/Gazebo clock"
+    )
+    declare_namespace_argument = DeclareLaunchArgument(
+        "namespace",
+        default_value="/",
+        description="Namespace for the explore node",
+    )
+    declare_known_init_poses_argument = DeclareLaunchArgument(
+        "known_init_poses",
+        default_value="True",
+        description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file",
+    )
+
+    # Map fully qualified names to relative ones so the node's namespace can be prepended.
+    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
+    # https://github.com/ros/geometry2/issues/32
+    # https://github.com/ros/robot_state_publisher/pull/30
+    remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
+
+    node = Node(
+        package="multirobot_map_merge",
+        name="map_merge",
+        namespace=namespace,
+        executable="map_merge",
+        parameters=[
+            config,
+            {"use_sim_time": use_sim_time},
+            {"known_init_poses": known_init_poses},
+        ],
+        output="screen",
+        remappings=remappings,
+    )
+    ld.add_action(declare_use_sim_time_argument)
+    ld.add_action(declare_known_init_poses_argument)
+    ld.add_action(declare_namespace_argument)
+    ld.add_action(node)
+    return ld
--- a/map_merge/launch/map_merge.rviz
+++ b/map_merge/launch/map_merge.rviz
@ -0,0 +1,154 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 1123
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map_updates
+      Use Timestamp: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 33.400630950927734
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.5996218919754028
+        Y: 0.5541106462478638
+        Z: -0.0447520911693573
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.1304056644439697
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1600
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000551fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000005510000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f000003aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000069000003ae0000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ae00000051fc0100000002fb0000000800540069006d00650100000000000004ae0000045300fffffffb0000000800540069006d00650100000000000004500000000000000000000002c60000055100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1198
+  X: 1322
+  Y: 115
--- a/map_merge/launch/tb3_simulation/bringup_launch.py
+++ b/map_merge/launch/tb3_simulation/bringup_launch.py
@ -0,0 +1,202 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    GroupAction,
+    IncludeLaunchDescription,
+    SetEnvironmentVariable,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import PushRosNamespace
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory("nav2_bringup")
+    launch_dir = os.path.join(bringup_dir, "launch")
+
+    # Get the launch directory of gmapping
+    slam_gmapping_dir = get_package_share_directory("slam_gmapping")
+    slam_gmapping_launch_dir = os.path.join(slam_gmapping_dir, "launch")
+
+    # Get the launch directory of map_merge
+    map_merge_dir = get_package_share_directory("multirobot_map_merge")
+    map_merge_launch_dir = os.path.join(map_merge_dir, "launch", "tb3_simulation")
+
+    # Create the launch configuration variables
+    namespace = LaunchConfiguration("namespace")
+    use_namespace = LaunchConfiguration("use_namespace")
+    slam = LaunchConfiguration("slam")
+    map_yaml_file = LaunchConfiguration("map")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    params_file = LaunchConfiguration("params_file")
+    autostart = LaunchConfiguration("autostart")
+    slam_toolbox = LaunchConfiguration("slam_toolbox")
+    slam_gmapping = LaunchConfiguration("slam_gmapping")
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        "RCUTILS_LOGGING_BUFFERED_STREAM", "1"
+    )
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        "namespace", default_value="", description="Top-level namespace"
+    )
+
+    declare_use_namespace_cmd = DeclareLaunchArgument(
+        "use_namespace",
+        default_value="false",
+        description="Whether to apply a namespace to the navigation stack",
+    )
+
+    declare_slam_cmd = DeclareLaunchArgument(
+        "slam", default_value="False", description="Whether run a SLAM"
+    )
+    declare_slam_toolbox_cmd = DeclareLaunchArgument(
+        "slam_toolbox", default_value="False", description="Whether run a SLAM toolbox"
+    )
+    declare_slam_gmapping_cmd = DeclareLaunchArgument(
+        "slam_gmapping",
+        default_value="False",
+        description="Whether run a SLAM gmapping",
+    )
+
+    declare_map_yaml_cmd = DeclareLaunchArgument(
+        "map", description="Full path to map yaml file to load"
+    )
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="false",
+        description="Use simulation (Gazebo) clock if true",
+    )
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        "params_file",
+        default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"),
+        description="Full path to the ROS2 parameters file to use for all launched nodes",
+    )
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        "autostart",
+        default_value="true",
+        description="Automatically startup the nav2 stack",
+    )
+
+    # Specify the actions
+    bringup_cmd_group = GroupAction(
+        [
+            PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace),
+            # IncludeLaunchDescription(
+            #     PythonLaunchDescriptionSource(
+            #         os.path.join(launch_dir, "slam_launch.py")
+            #     ),
+            #     condition=IfCondition(
+            #         PythonExpression(
+            #             [slam, " and ", slam_toolbox, " and not ", slam_gmapping]
+            #         )
+            #     ),
+            #     launch_arguments={
+            #         "namespace": namespace,
+            #         "use_sim_time": use_sim_time,
+            #         "autostart": autostart,
+            #         "params_file": params_file,
+            #     }.items(),
+            # ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(map_merge_launch_dir, "slam_toolbox.py")
+                ),
+                condition=IfCondition(
+                    PythonExpression(
+                        [slam, " and ", slam_toolbox, " and not ", slam_gmapping]
+                    )
+                ),
+                launch_arguments={
+                    "use_sim_time": use_sim_time,
+                }.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(launch_dir, "localization_launch.py")
+                ),
+                condition=IfCondition(PythonExpression(["not ", slam])),
+                launch_arguments={
+                    "namespace": namespace,
+                    "map": map_yaml_file,
+                    "use_sim_time": use_sim_time,
+                    "autostart": autostart,
+                    "params_file": params_file,
+                    "use_lifecycle_mgr": "false",
+                }.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(launch_dir, "navigation_launch.py")
+                ),
+                launch_arguments={
+                    "namespace": namespace,
+                    "use_sim_time": use_sim_time,
+                    "autostart": autostart,
+                    "params_file": params_file,
+                    "use_lifecycle_mgr": "false",
+                    "map_subscribe_transient_local": "true",
+                }.items(),
+            ),
+        ]
+    )
+    # Not in GroupAction because namespace were prepended twice because 
+    # slam_gmapping.launch.py already accepts a namespace argument
+    slam_gmapping_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(slam_gmapping_launch_dir, "slam_gmapping.launch.py")
+        ),
+        condition=IfCondition(
+            PythonExpression([slam, " and ", slam_gmapping, " and not ", slam_toolbox])
+        ),
+        launch_arguments={
+            "namespace": namespace,
+            "use_sim_time": use_sim_time,
+        }.items(),
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_namespace_cmd)
+    ld.add_action(declare_slam_cmd)
+    ld.add_action(declare_slam_toolbox_cmd)
+    ld.add_action(declare_slam_gmapping_cmd)
+    ld.add_action(declare_map_yaml_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(bringup_cmd_group)
+    ld.add_action(slam_gmapping_cmd)
+
+    return ld
--- a/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_1.yaml
+++ b/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_1.yaml
@ -0,0 +1,287 @@
+amcl:
+  ros__parameters:
+    use_sim_time: True
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "differential"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: True
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    # if enable_groot_monitoring is set to True, ports need to be different for each robot !!
+    enable_groot_monitoring: False
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugin: "goal_checker"
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    goal_checker:
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+      stateful: True
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 0.0
+      GoalAlign.scale: 0.0
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      use_sim_time: True
+      global_frame: odom
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["obstacle_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        scan:
+          topic: /robot1/scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+      observation_sources: scan
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: True
+      robot_radius: 0.22
+      obstacle_layer:
+        enabled: True
+        scan:
+          topic: /robot1/scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+      observation_sources: scan
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+map_server:
+  ros__parameters:
+    use_sim_time: True
+    yaml_filename: "turtlebot3_world.yaml"
+    save_map_timeout: 5.0
+
+planner_server:
+  ros__parameters:
+    use_sim_time: True
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    recovery_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_recoveries/Spin"
+    backup:
+      plugin: "nav2_recoveries/BackUp"
+    wait:
+      plugin: "nav2_recoveries/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_timeout: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
--- a/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_2.yaml
+++ b/map_merge/launch/tb3_simulation/config/nav2_multirobot_params_2.yaml
@ -0,0 +1,287 @@
+amcl:
+  ros__parameters:
+    use_sim_time: True
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "differential"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: True
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    # if enable_groot_monitoring is set to True, ports need to be different for each robot !!
+    enable_groot_monitoring: False
+    groot_zmq_publisher_port: 1789
+    groot_zmq_server_port: 1887
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugin: "goal_checker"
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    goal_checker:
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+      stateful: True
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 0.0
+      GoalAlign.scale: 0.0
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      use_sim_time: True
+      global_frame: odom
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["obstacle_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        scan:
+          topic: /robot2/scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+      observation_sources: scan
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: True
+      robot_radius: 0.22
+      obstacle_layer:
+        enabled: True
+        scan:
+          topic: /robot2/scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+      observation_sources: scan
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+map_server:
+  ros__parameters:
+    use_sim_time: True
+    yaml_filename: "turtlebot3_world.yaml"
+    save_map_timeout: 5.0
+
+planner_server:
+  ros__parameters:
+    use_sim_time: True
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    recovery_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_recoveries/Spin"
+    backup:
+      plugin: "nav2_recoveries/BackUp"
+    wait:
+      plugin: "nav2_recoveries/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_timeout: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
--- a/map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py
+++ b/map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py
@ -0,0 +1,293 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""
+Example for spawing multiple robots in Gazebo.
+
+This is an example on how to create a launch file for spawning multiple robots into Gazebo
+and launch multiple instances of the navigation stack, each controlling one robot.
+The robots co-exist on a shared environment and are controlled by independent nav stacks
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription, condition
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    GroupAction,
+    IncludeLaunchDescription,
+    LogInfo,
+)
+from launch.conditions import IfCondition, UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory("nav2_bringup")
+    launch_dir = os.path.join(bringup_dir, "launch")
+
+    # Get the launch directory for multirobot_map_merge where we have a modified launch files
+    map_merge_dir = get_package_share_directory("multirobot_map_merge")
+    launch_dir_map_merge = os.path.join(map_merge_dir, "launch", "tb3_simulation")
+
+    # Names and poses of the robots for known poses demo
+    robots_known_poses = [
+        {"name": "robot1", "x_pose": 0.0, "y_pose": 0.5, "z_pose": 0.01},
+        {"name": "robot2", "x_pose": -3.0, "y_pose": 1.5, "z_pose": 0.01},
+    ]
+    # Names and poses of the robots for unknown poses demo, the must be very close at beggining
+    robots_unknown_poses = [
+        {"name": "robot1", "x_pose": -2.0, "y_pose": 0.5, "z_pose": 0.01},
+        {"name": "robot2", "x_pose": -3.0, "y_pose": 0.5, "z_pose": 0.01},
+    ]
+
+    # Simulation settings
+    world = LaunchConfiguration("world")
+    simulator = LaunchConfiguration("simulator")
+
+    # On this example all robots are launched with the same settings
+    map_yaml_file = LaunchConfiguration("map")
+
+    autostart = LaunchConfiguration("autostart")
+    rviz_config_file = LaunchConfiguration("rviz_config")
+    use_robot_state_pub = LaunchConfiguration("use_robot_state_pub")
+    use_rviz = LaunchConfiguration("use_rviz")
+    log_settings = LaunchConfiguration("log_settings", default="true")
+
+    known_init_poses = LaunchConfiguration("known_init_poses")
+    declare_known_init_poses_cmd = DeclareLaunchArgument(
+        "known_init_poses",
+        default_value="True",
+        description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file",
+    )
+
+    # Declare the launch arguments
+    declare_world_cmd = DeclareLaunchArgument(
+        "world",
+        default_value=os.path.join(launch_dir_map_merge, "worlds", "world_only.model"),
+        description="Full path to world file to load",
+    )
+
+    declare_simulator_cmd = DeclareLaunchArgument(
+        "simulator",
+        default_value="gazebo",
+        description="The simulator to use (gazebo or gzserver)",
+    )
+
+    declare_map_yaml_cmd = DeclareLaunchArgument(
+        "map",
+        default_value=os.path.join(bringup_dir, "maps", "turtlebot3_world.yaml"),
+        description="Full path to map file to load",
+    )
+
+    declare_robot1_params_file_cmd = DeclareLaunchArgument(
+        "robot1_params_file",
+        default_value=os.path.join(
+            launch_dir_map_merge, "config", "nav2_multirobot_params_1.yaml"
+        ),
+        description="Full path to the ROS2 parameters file to use for robot1 launched nodes",
+    )
+
+    declare_robot2_params_file_cmd = DeclareLaunchArgument(
+        "robot2_params_file",
+        default_value=os.path.join(
+            launch_dir_map_merge, "config", "nav2_multirobot_params_2.yaml"
+        ),
+        description="Full path to the ROS2 parameters file to use for robot2 launched nodes",
+    )
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        "autostart",
+        default_value="true",
+        description="Automatically startup the stacks",
+    )
+
+    declare_rviz_config_file_cmd = DeclareLaunchArgument(
+        "rviz_config",
+        default_value=os.path.join(bringup_dir, "rviz", "nav2_namespaced_view.rviz"),
+        description="Full path to the RVIZ config file to use.",
+    )
+
+    declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
+        "use_robot_state_pub",
+        default_value="True",
+        description="Whether to start the robot state publisher",
+    )
+
+    declare_use_rviz_cmd = DeclareLaunchArgument(
+        "use_rviz", default_value="True", description="Whether to start RVIZ"
+    )
+
+    slam_toolbox = LaunchConfiguration("slam_toolbox")
+    slam_gmapping = LaunchConfiguration("slam_gmapping")
+    declare_slam_toolbox_cmd = DeclareLaunchArgument(
+        "slam_toolbox", default_value="False", description="Whether run a SLAM toolbox"
+    )
+    declare_slam_gmapping_cmd = DeclareLaunchArgument(
+        "slam_gmapping",
+        default_value="False",
+        description="Whether run a SLAM gmapping",
+    )
+
+    # Start Gazebo with plugin providing the robot spawing service
+    start_gazebo_cmd = ExecuteProcess(
+        cmd=[
+            simulator,
+            "--verbose",
+            "-s",
+            "libgazebo_ros_init.so",
+            "-s",
+            "libgazebo_ros_factory.so",
+            world,
+        ],
+        output="screen",
+    )
+
+    # Define commands for spawing the robots into Gazebo
+    spawn_robots_cmds = []
+    for robot_known, robot_unknown in zip(robots_known_poses, robots_unknown_poses):
+        spawn_robots_cmds.append(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
+                ),
+                launch_arguments={
+                    "x_pose": TextSubstitution(text=str(robot_known["x_pose"])),
+                    "y_pose": TextSubstitution(text=str(robot_known["y_pose"])),
+                    "z_pose": TextSubstitution(text=str(robot_known["z_pose"])),
+                    "robot_name": robot_known["name"],
+                    "turtlebot_type": TextSubstitution(text="waffle"),
+                }.items(),
+                condition=IfCondition(known_init_poses),
+            )
+        )
+        spawn_robots_cmds.append(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
+                ),
+                launch_arguments={
+                    "x_pose": TextSubstitution(text=str(robot_unknown["x_pose"])),
+                    "y_pose": TextSubstitution(text=str(robot_unknown["y_pose"])),
+                    "z_pose": TextSubstitution(text=str(robot_unknown["z_pose"])),
+                    "robot_name": robot_unknown["name"],
+                    "turtlebot_type": TextSubstitution(text="waffle"),
+                }.items(),
+                condition=UnlessCondition(known_init_poses),
+            )
+        )
+
+    # Define commands for launching the navigation instances
+    nav_instances_cmds = []
+    for robot in robots_known_poses:
+        params_file = LaunchConfiguration(f"{robot['name']}_params_file")
+
+        group = GroupAction(
+            [
+                IncludeLaunchDescription(
+                    PythonLaunchDescriptionSource(
+                        os.path.join(launch_dir, "rviz_launch.py")
+                    ),
+                    condition=IfCondition(use_rviz),
+                    launch_arguments={
+                        "namespace": TextSubstitution(text=robot["name"]),
+                        "use_namespace": "True",
+                        "rviz_config": rviz_config_file,
+                    }.items(),
+                ),
+                IncludeLaunchDescription(
+                    PythonLaunchDescriptionSource(
+                        os.path.join(launch_dir_map_merge, "tb3_simulation_launch.py")
+                    ),
+                    launch_arguments={
+                        "namespace": robot["name"],
+                        "use_namespace": "True",
+                        "map": map_yaml_file,
+                        "use_sim_time": "True",
+                        "params_file": params_file,
+                        "autostart": autostart,
+                        "use_rviz": "False",
+                        "use_simulator": "False",
+                        "headless": "False",
+                        "slam": "True",
+                        "slam_toolbox": slam_toolbox,
+                        "slam_gmapping": slam_gmapping,
+                        "use_robot_state_pub": use_robot_state_pub,
+                    }.items(),
+                ),
+                LogInfo(
+                    condition=IfCondition(log_settings),
+                    msg=["Launching ", robot["name"]],
+                ),
+                LogInfo(
+                    condition=IfCondition(log_settings),
+                    msg=[robot["name"], " map yaml: ", map_yaml_file],
+                ),
+                LogInfo(
+                    condition=IfCondition(log_settings),
+                    msg=[robot["name"], " params yaml: ", params_file],
+                ),
+                LogInfo(
+                    condition=IfCondition(log_settings),
+                    msg=[robot["name"], " rviz config file: ", rviz_config_file],
+                ),
+                LogInfo(
+                    condition=IfCondition(log_settings),
+                    msg=[
+                        robot["name"],
+                        " using robot state pub: ",
+                        use_robot_state_pub,
+                    ],
+                ),
+                LogInfo(
+                    condition=IfCondition(log_settings),
+                    msg=[robot["name"], " autostart: ", autostart],
+                ),
+            ]
+        )
+
+        nav_instances_cmds.append(group)
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_simulator_cmd)
+    ld.add_action(declare_world_cmd)
+    ld.add_action(declare_map_yaml_cmd)
+    ld.add_action(declare_robot1_params_file_cmd)
+    ld.add_action(declare_robot2_params_file_cmd)
+    ld.add_action(declare_use_rviz_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_rviz_config_file_cmd)
+    ld.add_action(declare_use_robot_state_pub_cmd)
+    ld.add_action(declare_slam_toolbox_cmd)
+    ld.add_action(declare_slam_gmapping_cmd)
+    ld.add_action(declare_known_init_poses_cmd)
+
+    # Add the actions to start gazebo, robots and simulations
+    ld.add_action(start_gazebo_cmd)
+
+    for spawn_robot_cmd in spawn_robots_cmds:
+        ld.add_action(spawn_robot_cmd)
+
+    for simulation_instance_cmd in nav_instances_cmds:
+        ld.add_action(simulation_instance_cmd)
+
+    return ld
--- a/map_merge/launch/tb3_simulation/slam_toolbox.py
+++ b/map_merge/launch/tb3_simulation/slam_toolbox.py
@ -0,0 +1,49 @@
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    slam_params_file = LaunchConfiguration("slam_params_file")
+
+    remappings = [
+        ("/map", "map"),
+        ("/scan", "scan"),
+        ("/tf", "tf"),
+        ("/tf_static", "tf_static"),
+    ]
+
+    declare_use_sim_time_argument = DeclareLaunchArgument(
+        "use_sim_time", default_value="true", description="Use simulation/Gazebo clock"
+    )
+    declare_slam_params_file_cmd = DeclareLaunchArgument(
+        "slam_params_file",
+        default_value=os.path.join(
+            get_package_share_directory("slam_toolbox"),
+            "config",
+            "mapper_params_online_sync.yaml",
+        ),
+        description="Full path to the ROS2 parameters file to use for the slam_toolbox node",
+    )
+
+    start_sync_slam_toolbox_node = Node(
+        parameters=[slam_params_file, {"use_sim_time": use_sim_time}],
+        package="slam_toolbox",
+        executable="sync_slam_toolbox_node",
+        name="slam_toolbox",
+        output="screen",
+        remappings=remappings,
+    )
+
+    ld = LaunchDescription()
+
+    ld.add_action(declare_use_sim_time_argument)
+    ld.add_action(declare_slam_params_file_cmd)
+    ld.add_action(start_sync_slam_toolbox_node)
+
+    return ld
--- a/map_merge/launch/tb3_simulation/tb3_simulation_launch.py
+++ b/map_merge/launch/tb3_simulation/tb3_simulation_launch.py
@ -0,0 +1,245 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""This is all-in-one launch script intended for use by nav2 developers."""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory("nav2_bringup")
+    launch_dir = os.path.join(bringup_dir, "launch")
+
+    # Get the launch directory for multirobot_map_merge where we have a modified bringup launch file
+    map_merge_dir = get_package_share_directory("multirobot_map_merge")
+    launch_dir_map_merge = os.path.join(map_merge_dir, "launch", "tb3_simulation")
+
+    # Create the launch configuration variables
+    slam = LaunchConfiguration("slam")
+    slam_toolbox = LaunchConfiguration("slam_toolbox")
+    slam_gmapping = LaunchConfiguration("slam_gmapping")
+    namespace = LaunchConfiguration("namespace")
+    use_namespace = LaunchConfiguration("use_namespace")
+    map_yaml_file = LaunchConfiguration("map")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    params_file = LaunchConfiguration("params_file")
+    autostart = LaunchConfiguration("autostart")
+
+    # Launch configuration variables specific to simulation
+    rviz_config_file = LaunchConfiguration("rviz_config_file")
+    use_simulator = LaunchConfiguration("use_simulator")
+    use_robot_state_pub = LaunchConfiguration("use_robot_state_pub")
+    use_rviz = LaunchConfiguration("use_rviz")
+    headless = LaunchConfiguration("headless")
+    world = LaunchConfiguration("world")
+
+    # Map fully qualified names to relative ones so the node's namespace can be prepended.
+    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
+    # https://github.com/ros/geometry2/issues/32
+    # https://github.com/ros/robot_state_publisher/pull/30
+    # TODO(orduno) Substitute with `PushNodeRemapping`
+    #              https://github.com/ros2/launch_ros/issues/56
+    remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
+
+    # Declare the launch arguments
+    declare_namespace_cmd = DeclareLaunchArgument(
+        "namespace", default_value="", description="Top-level namespace"
+    )
+
+    declare_use_namespace_cmd = DeclareLaunchArgument(
+        "use_namespace",
+        default_value="false",
+        description="Whether to apply a namespace to the navigation stack",
+    )
+
+    declare_slam_cmd = DeclareLaunchArgument(
+        "slam", default_value="False", description="Whether run a SLAM"
+    )
+    declare_slam_toolbox_cmd = DeclareLaunchArgument(
+        "slam_toolbox", default_value="False", description="Whether run a SLAM toolbox"
+    )
+    declare_slam_gmapping_cmd = DeclareLaunchArgument(
+        "slam_gmapping",
+        default_value="False",
+        description="Whether run a SLAM gmapping",
+    )
+
+    declare_map_yaml_cmd = DeclareLaunchArgument(
+        "map",
+        default_value=os.path.join(bringup_dir, "maps", "turtlebot3_world.yaml"),
+        description="Full path to map file to load",
+    )
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="true",
+        description="Use simulation (Gazebo) clock if true",
+    )
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        "params_file",
+        default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"),
+        description="Full path to the ROS2 parameters file to use for all launched nodes",
+    )
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        "autostart",
+        default_value="true",
+        description="Automatically startup the nav2 stack",
+    )
+
+    declare_rviz_config_file_cmd = DeclareLaunchArgument(
+        "rviz_config_file",
+        default_value=os.path.join(bringup_dir, "rviz", "nav2_default_view.rviz"),
+        description="Full path to the RVIZ config file to use",
+    )
+
+    declare_use_simulator_cmd = DeclareLaunchArgument(
+        "use_simulator",
+        default_value="True",
+        description="Whether to start the simulator",
+    )
+
+    declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
+        "use_robot_state_pub",
+        default_value="True",
+        description="Whether to start the robot state publisher",
+    )
+
+    declare_use_rviz_cmd = DeclareLaunchArgument(
+        "use_rviz", default_value="True", description="Whether to start RVIZ"
+    )
+
+    declare_simulator_cmd = DeclareLaunchArgument(
+        "headless", default_value="False", description="Whether to execute gzclient)"
+    )
+
+    declare_world_cmd = DeclareLaunchArgument(
+        "world",
+        # TODO(orduno) Switch back once ROS argument passing has been fixed upstream
+        #              https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
+        # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
+        # worlds/turtlebot3_worlds/waffle.model')
+        default_value=os.path.join(bringup_dir, "worlds", "waffle.model"),
+        description="Full path to world model file to load",
+    )
+
+    # Specify the actions
+    start_gazebo_server_cmd = ExecuteProcess(
+        condition=IfCondition(use_simulator),
+        cmd=[
+            "gzserver",
+            "-s",
+            "libgazebo_ros_init.so",
+            "-s",
+            "libgazebo_ros_factory.so",
+            world,
+        ],
+        cwd=[launch_dir],
+        output="screen",
+    )
+
+    start_gazebo_client_cmd = ExecuteProcess(
+        condition=IfCondition(PythonExpression([use_simulator, " and not ", headless])),
+        cmd=["gzclient"],
+        cwd=[launch_dir],
+        output="screen",
+    )
+
+    urdf = os.path.join(bringup_dir, "urdf", "turtlebot3_waffle.urdf")
+
+    start_robot_state_publisher_cmd = Node(
+        condition=IfCondition(use_robot_state_pub),
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        namespace=namespace,
+        output="screen",
+        parameters=[{"use_sim_time": use_sim_time}],
+        remappings=remappings,
+        arguments=[urdf],
+    )
+
+    rviz_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(launch_dir, "rviz_launch.py")),
+        condition=IfCondition(use_rviz),
+        launch_arguments={
+            "namespace": "",
+            "use_namespace": "False",
+            "rviz_config": rviz_config_file,
+        }.items(),
+    )
+
+    bringup_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_dir_map_merge, "bringup_launch.py")
+        ),
+        launch_arguments={
+            "namespace": namespace,
+            "use_namespace": use_namespace,
+            "slam": slam,
+            "slam_toolbox": slam_toolbox,
+            "slam_gmapping": slam_gmapping,
+            "map": map_yaml_file,
+            "use_sim_time": use_sim_time,
+            "params_file": params_file,
+            "autostart": autostart,
+        }.items(),
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_namespace_cmd)
+    ld.add_action(declare_slam_cmd)
+    ld.add_action(declare_slam_toolbox_cmd)
+    ld.add_action(declare_slam_gmapping_cmd)
+    ld.add_action(declare_map_yaml_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+
+    ld.add_action(declare_rviz_config_file_cmd)
+    ld.add_action(declare_use_simulator_cmd)
+    ld.add_action(declare_use_robot_state_pub_cmd)
+    ld.add_action(declare_use_rviz_cmd)
+    ld.add_action(declare_simulator_cmd)
+    ld.add_action(declare_world_cmd)
+
+    # Add any conditioned actions
+    ld.add_action(start_gazebo_server_cmd)
+    ld.add_action(start_gazebo_client_cmd)
+
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(start_robot_state_publisher_cmd)
+    ld.add_action(rviz_cmd)
+    ld.add_action(bringup_cmd)
+
+    return ld
--- a/map_merge/launch/tb3_simulation/worlds/world_only.model
+++ b/map_merge/launch/tb3_simulation/worlds/world_only.model
@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <scene>
+      <shadows>false</shadows>
+    </scene>
+
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+
+    <physics type="ode">
+      <real_time_update_rate>1000.0</real_time_update_rate>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>150</iters>
+          <precon_iters>0</precon_iters>
+          <sor>1.400000</sor>
+          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+        </solver>
+        <constraints>
+          <cfm>0.00001</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
+          <contact_surface_layer>0.01000</contact_surface_layer>
+        </constraints>
+      </ode>
+    </physics>
+
+    <model name="turtlebot3_world">
+      <static>1</static>
+      <include>
+        <!-- <uri>model://turtlebot3_dqn_world</uri> -->
+        <!-- <uri>model://turtlebot3_world</uri> -->
+        <uri>model://turtlebot3_house</uri>
+      </include>
+    </model>
+
+  </world>
+</sdf>
--- a/map_merge/package.xml
+++ b/map_merge/package.xml
@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>multirobot_map_merge</name>
+  <version>1.0.0</version>
+
+  <description>Merging multiple maps without knowledge of initial
+  positions of robots ROS2 Port.</description>
+
+  <author email="candres.alv@gmail.com">Carlos Alvarez</author>
+  <maintainer email="candres.alv@gmail.com">Carlos Alvarez</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <depend>ament_cmake</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>map_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <!-- used to get OpenCV -->
+  <depend>image_geometry</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/map_merge/src/combine_grids/estimation_internal.h
+++ b/map_merge/src/combine_grids/estimation_internal.h
@ -0,0 +1,152 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#ifndef ESTIMATION_INTERNAL_H_
+#define ESTIMATION_INTERNAL_H_
+
+#include <combine_grids/merging_pipeline.h>
+
+#include <iostream>
+#include <cassert>
+#include <opencv2/core/utility.hpp>
+#include <opencv2/core/version.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/stitching/detail/matchers.hpp>
+
+#ifdef HAVE_OPENCV_XFEATURES2D
+#include <opencv2/xfeatures2d/nonfree.hpp>
+#endif
+
+namespace combine_grids
+{
+namespace internal
+{
+#if CV_VERSION_MAJOR >= 4
+
+static inline cv::Ptr<cv::Feature2D> chooseFeatureFinder(FeatureType type)
+{
+  switch (type) {
+    case FeatureType::AKAZE:
+      return cv::AKAZE::create();
+    case FeatureType::ORB:
+      return cv::ORB::create();
+    case FeatureType::SURF:
+#ifdef HAVE_OPENCV_XFEATURES2D
+      return cv::xfeatures2d::SURF::create();
+#else
+      return cv::AKAZE::create();
+#endif
+  }
+
+  assert(false);
+  return {};
+}
+
+#else  // (CV_VERSION_MAJOR < 4)
+
+static inline cv::Ptr<cv::detail::FeaturesFinder>
+chooseFeatureFinder(FeatureType type)
+{
+  switch (type) {
+    case FeatureType::AKAZE:
+      return cv::makePtr<cv::detail::AKAZEFeaturesFinder>();
+    case FeatureType::ORB:
+      return cv::makePtr<cv::detail::OrbFeaturesFinder>();
+    case FeatureType::SURF:
+      return cv::makePtr<cv::detail::SurfFeaturesFinder>();
+  }
+
+  assert(false);
+  return {};
+}
+
+#endif  // CV_VERSION_MAJOR >= 4
+
+static inline void writeDebugMatchingInfo(
+    const std::vector<cv::Mat>& images,
+    const std::vector<cv::detail::ImageFeatures>& image_features,
+    const std::vector<cv::detail::MatchesInfo>& pairwise_matches)
+{
+  for (auto& match_info : pairwise_matches) {
+    if (match_info.H.empty() ||
+        match_info.src_img_idx >= match_info.dst_img_idx) {
+      continue;
+    }
+    std::cout << match_info.src_img_idx << " " << match_info.dst_img_idx
+              << std::endl
+              << "features: "
+              << image_features[size_t(match_info.src_img_idx)].keypoints.size()
+              << " "
+              << image_features[size_t(match_info.dst_img_idx)].keypoints.size()
+              << std::endl
+              << "matches: " << match_info.matches.size() << std::endl
+              << "inliers: " << match_info.num_inliers << std::endl
+              << "inliers/matches ratio: "
+              << match_info.num_inliers / double(match_info.matches.size())
+              << std::endl
+              << "confidence: " << match_info.confidence << std::endl
+              << match_info.H << std::endl;
+    cv::Mat img;
+    // draw all matches
+    cv::drawMatches(images[size_t(match_info.src_img_idx)],
+                    image_features[size_t(match_info.src_img_idx)].keypoints,
+                    images[size_t(match_info.dst_img_idx)],
+                    image_features[size_t(match_info.dst_img_idx)].keypoints,
+                    match_info.matches, img);
+    cv::imwrite(std::to_string(match_info.src_img_idx) + "_" +
+                    std::to_string(match_info.dst_img_idx) + "_matches.png",
+                img);
+    // draw inliers only
+    cv::drawMatches(
+        images[size_t(match_info.src_img_idx)],
+        image_features[size_t(match_info.src_img_idx)].keypoints,
+        images[size_t(match_info.dst_img_idx)],
+        image_features[size_t(match_info.dst_img_idx)].keypoints,
+        match_info.matches, img, cv::Scalar::all(-1), cv::Scalar::all(-1),
+        *reinterpret_cast<const std::vector<char>*>(&match_info.inliers_mask));
+    cv::imwrite(std::to_string(match_info.src_img_idx) + "_" +
+                    std::to_string(match_info.dst_img_idx) +
+                    "_matches_inliers.png",
+                img);
+  }
+}
+
+}  // namespace internal
+}  // namespace combine_grids
+
+#endif  // ESTIMATION_INTERNAL_H_
--- a/map_merge/src/combine_grids/grid_compositor.cpp
+++ b/map_merge/src/combine_grids/grid_compositor.cpp
@ -0,0 +1,88 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#include <combine_grids/grid_compositor.h>
+
+#include <opencv2/stitching/detail/util.hpp>
+
+#include <rcpputils/asserts.hpp>
+
+
+namespace combine_grids
+{
+namespace internal
+{
+nav_msgs::msg::OccupancyGrid::SharedPtr GridCompositor::compose(
+    const std::vector<cv::Mat>& grids, const std::vector<cv::Rect>& rois)
+{
+  rcpputils::require_true(grids.size() == rois.size());
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr result_grid(new nav_msgs::msg::OccupancyGrid());
+
+  std::vector<cv::Point> corners;
+  corners.reserve(grids.size());
+  std::vector<cv::Size> sizes;
+  sizes.reserve(grids.size());
+  for (auto& roi : rois) {
+    corners.push_back(roi.tl());
+    sizes.push_back(roi.size());
+  }
+  cv::Rect dst_roi = cv::detail::resultRoi(corners, sizes);
+
+  result_grid->info.width = static_cast<uint>(dst_roi.width);
+  result_grid->info.height = static_cast<uint>(dst_roi.height);
+  result_grid->data.resize(static_cast<size_t>(dst_roi.area()), -1);
+  // create view for opencv pointing to newly allocated grid
+  cv::Mat result(dst_roi.size(), CV_8S, result_grid->data.data());
+
+  for (size_t i = 0; i < grids.size(); ++i) {
+    // we need to compensate global offset
+    cv::Rect roi = cv::Rect(corners[i] - dst_roi.tl(), sizes[i]);
+    cv::Mat result_roi(result, roi);
+    // reinterpret warped matrix as signed
+    // we will not change this matrix, but opencv does not support const matrices
+    cv::Mat warped_signed (grids[i].size(), CV_8S, const_cast<uchar*>(grids[i].ptr()));
+    // compose img into result matrix
+    cv::max(result_roi, warped_signed, result_roi);
+  }
+
+  return result_grid;
+}
+
+}  // namespace internal
+
+}  // namespace combine_grids
--- a/map_merge/src/combine_grids/grid_warper.cpp
+++ b/map_merge/src/combine_grids/grid_warper.cpp
@ -0,0 +1,90 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#include <combine_grids/grid_warper.h>
+
+#include <opencv2/stitching/detail/warpers.hpp>
+
+#include <rcpputils/asserts.hpp>
+
+namespace combine_grids
+{
+namespace internal
+{
+cv::Rect GridWarper::warp(const cv::Mat& grid, const cv::Mat& transform,
+                          cv::Mat& warped_grid)
+{
+  // for validating function inputs. Throws an std::invalid_argument exception if the condition fails.
+  rcpputils::require_true(transform.type() == CV_64F);
+  cv::Mat H;
+  invertAffineTransform(transform.rowRange(0, 2), H);
+  cv::Rect roi = warpRoi(grid, H);
+  // shift top left corner for warp affine (otherwise the image is cropped)
+  H.at<double>(0, 2) -= roi.tl().x;
+  H.at<double>(1, 2) -= roi.tl().y;
+  warpAffine(grid, warped_grid, H, roi.size(), cv::INTER_NEAREST,
+             cv::BORDER_CONSTANT,
+             cv::Scalar::all(255) /* this is -1 for signed char */);
+  
+  // For verifying results. Throws a rcpputils::AssertionException if the condition fails. 
+  // This function becomes a no-op in release builds.
+  rcpputils::assert_true(roi.size() == warped_grid.size());
+
+  return roi;
+}
+
+cv::Rect GridWarper::warpRoi(const cv::Mat& grid, const cv::Mat& transform)
+{
+  cv::Ptr<cv::detail::PlaneWarper> warper =
+      cv::makePtr<cv::detail::PlaneWarper>();
+  cv::Mat H;
+  transform.convertTo(H, CV_32F);
+
+  // separate rotation and translation for plane warper
+  // 3D translation
+  cv::Mat T = cv::Mat::zeros(3, 1, CV_32F);
+  H.colRange(2, 3).rowRange(0, 2).copyTo(T.rowRange(0, 2));
+  // 3D rotation
+  cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
+  H.colRange(0, 2).copyTo(R.rowRange(0, 2).colRange(0, 2));
+
+  return warper->warpRoi(grid.size(), cv::Mat::eye(3, 3, CV_32F), R, T);
+}
+
+}  // namespace internal
+
+}  // namespace combine_grids
--- a/map_merge/src/combine_grids/merging_pipeline.cpp
+++ b/map_merge/src/combine_grids/merging_pipeline.cpp
@ -0,0 +1,280 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#include <combine_grids/grid_compositor.h>
+#include <combine_grids/grid_warper.h>
+#include <combine_grids/merging_pipeline.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rcpputils/asserts.hpp>
+
+#include <opencv2/stitching/detail/matchers.hpp>
+#include <opencv2/stitching/detail/motion_estimators.hpp>
+
+#include "estimation_internal.h"
+
+namespace combine_grids
+{
+bool MergingPipeline::estimateTransforms(FeatureType feature_type,
+                                         double confidence)
+{
+  std::vector<cv::detail::ImageFeatures> image_features;
+  std::vector<cv::detail::MatchesInfo> pairwise_matches;
+  std::vector<cv::detail::CameraParams> transforms;
+  std::vector<int> good_indices;
+  // TODO investigate value translation effect on features
+  auto finder = internal::chooseFeatureFinder(feature_type);
+  cv::Ptr<cv::detail::FeaturesMatcher> matcher =
+      cv::makePtr<cv::detail::AffineBestOf2NearestMatcher>();
+  cv::Ptr<cv::detail::Estimator> estimator =
+      cv::makePtr<cv::detail::AffineBasedEstimator>();
+  cv::Ptr<cv::detail::BundleAdjusterBase> adjuster =
+      cv::makePtr<cv::detail::BundleAdjusterAffinePartial>();
+
+  if (images_.empty()) {
+    return true;
+  }
+
+  /* find features in images */
+  static rclcpp::Logger logger = rclcpp::get_logger("estimateTransforms");
+  RCLCPP_DEBUG(logger, "computing features");
+  image_features.reserve(images_.size());
+  for (const cv::Mat& image : images_) {
+    image_features.emplace_back();
+    if (!image.empty()) {
+#if CV_VERSION_MAJOR >= 4
+      cv::detail::computeImageFeatures(finder, image, image_features.back());
+#else
+      (*finder)(image, image_features.back());
+#endif
+    }
+  }
+  finder = {};
+
+  /* find corespondent features */
+  RCLCPP_DEBUG(logger, "pairwise matching features");
+  (*matcher)(image_features, pairwise_matches);
+  matcher = {};
+
+#ifndef NDEBUG
+  internal::writeDebugMatchingInfo(images_, image_features, pairwise_matches);
+#endif
+
+  /* use only matches that has enough confidence. leave out matches that are not
+   * connected (small components) */
+  good_indices = cv::detail::leaveBiggestComponent(
+      image_features, pairwise_matches, static_cast<float>(confidence));
+
+  // no match found. try set first non-empty grid as reference frame. we try to
+  // avoid setting empty grid as reference frame, in case some maps never
+  // arrive. If all is empty just set null transforms.
+  // if (good_indices.size() == 1) {
+  //   transforms_.clear();
+  //   transforms_.resize(images_.size());
+  //   for (size_t i = 0; i < images_.size(); ++i) {
+  //     if (!images_[i].empty()) {
+  //       // set identity
+  //       transforms_[i] = cv::Mat::eye(3, 3, CV_64F);
+  //       break;
+  //     }
+  //   }
+  //   // RCLCPP_INFO(logger, "No match found between maps, setting first non-empty grid as reference frame");
+  //   return true;
+  // }
+
+  // Making some tests it is better to just return false if no match is found
+  // and not clear the last good transforms found
+  if (good_indices.size() == 1) {
+    if (images_.size() != transforms_.size()) {
+      transforms_.clear();
+      transforms_.resize(images_.size());
+    }
+    return false;
+  }
+
+  // // Experimental: should we keep only the best confidence match overall?
+  // bool max_confidence_achieved_surpassed = false;
+  // for (auto &match_info : pairwise_matches) {
+  //   RCLCPP_INFO(logger, "match info: %f", match_info.confidence);
+  //   if (match_info.confidence > max_confidence_achieved_){
+  //     max_confidence_achieved_surpassed = true;
+  //     max_confidence_achieved_ = match_info.confidence;
+  //   }
+  // }
+  // if (!max_confidence_achieved_surpassed) {
+  //   RCLCPP_INFO(logger, "Max confidence achieved not surpassed, not using matching");
+  //   return false;
+  // }
+  // else
+  //   RCLCPP_INFO(logger, "Max confidence achieved surpassed, optimizing");
+
+
+  /* estimate transform */
+  RCLCPP_DEBUG(logger, "calculating transforms in global reference frame");
+  // note: currently used estimator never fails
+  if (!(*estimator)(image_features, pairwise_matches, transforms)) {
+    return false;
+  }
+
+  /* levmarq optimization */
+  // openCV just accepts float transforms
+  for (auto& transform : transforms) {
+    transform.R.convertTo(transform.R, CV_32F);
+  }
+  RCLCPP_DEBUG(logger, "optimizing global transforms");
+  adjuster->setConfThresh(confidence);
+  if (!(*adjuster)(image_features, pairwise_matches, transforms)) {
+    RCLCPP_WARN(logger, "Bundle adjusting failed. Could not estimate transforms.");
+    return false;
+  }
+
+  transforms_.clear();
+  transforms_.resize(images_.size());
+  size_t i = 0;
+  for (auto& j : good_indices) {
+    // we want to work with transforms as doubles
+    transforms[i].R.convertTo(transforms_[static_cast<size_t>(j)], CV_64F);
+    ++i;
+  }
+
+  return true;
+}
+
+// checks whether given matrix is an identity, i.e. exactly appropriate Mat::eye
+static inline bool isIdentity(const cv::Mat& matrix)
+{
+  if (matrix.empty()) {
+    return false;
+  }
+  cv::MatExpr diff = matrix != cv::Mat::eye(matrix.size(), matrix.type());
+  return cv::countNonZero(diff) == 0;
+}
+
+nav_msgs::msg::OccupancyGrid::SharedPtr MergingPipeline::composeGrids()
+{
+  // for checking states. Throws a rcpputils::IllegalStateException if the condition fails.
+  rcpputils::check_true(images_.size() == transforms_.size());
+  rcpputils::check_true(images_.size() == grids_.size());
+  static rclcpp::Logger logger = rclcpp::get_logger("composeGrids");
+
+  if (images_.empty()) {
+    return nullptr;
+  }
+
+  RCLCPP_DEBUG(logger, "warping grids");
+  internal::GridWarper warper;
+  std::vector<cv::Mat> imgs_warped;
+  imgs_warped.reserve(images_.size());
+  std::vector<cv::Rect> rois;
+  rois.reserve(images_.size());
+
+  for (size_t i = 0; i < images_.size(); ++i) {
+    if (!transforms_[i].empty() && !images_[i].empty()) {
+      imgs_warped.emplace_back();
+      rois.emplace_back(
+          warper.warp(images_[i], transforms_[i], imgs_warped.back()));
+    }
+  }
+
+  if (imgs_warped.empty()) {
+    return nullptr;
+  }
+
+  RCLCPP_DEBUG(logger, "compositing result grid");
+  nav_msgs::msg::OccupancyGrid::SharedPtr result;
+  internal::GridCompositor compositor;
+  result = compositor.compose(imgs_warped, rois);
+
+  // set correct resolution to output grid. use resolution of identity (works
+  // for estimated transforms), or any resolution (works for know_init_positions)
+  // - in that case all resolutions should be the same.
+  float any_resolution = 0.0;
+  for (size_t i = 0; i < transforms_.size(); ++i) {
+    // check if this transform is the reference frame
+    if (isIdentity(transforms_[i])) {
+      result->info.resolution = grids_[i]->info.resolution;
+      break;
+    }
+    if (grids_[i]) {
+      any_resolution = grids_[i]->info.resolution;
+    }
+  }
+  if (result->info.resolution <= 0.f) {
+    result->info.resolution = any_resolution;
+  }
+
+  // set grid origin to its centre
+  result->info.origin.position.x =
+      -(result->info.width / 2.0) * double(result->info.resolution);
+  result->info.origin.position.y =
+      -(result->info.height / 2.0) * double(result->info.resolution);
+  result->info.origin.orientation.w = 1.0;
+
+  return result;
+}
+
+std::vector<geometry_msgs::msg::Transform> MergingPipeline::getTransforms() const
+{
+  std::vector<geometry_msgs::msg::Transform> result;
+  result.reserve(transforms_.size());
+
+  for (auto& transform : transforms_) {
+    if (transform.empty()) {
+      result.emplace_back();
+      continue;
+    }
+
+    rcpputils::require_true(transform.type() == CV_64F);
+    geometry_msgs::msg::Transform ros_transform;
+    ros_transform.translation.x = transform.at<double>(0, 2);
+    ros_transform.translation.y = transform.at<double>(1, 2);
+    ros_transform.translation.z = 0.;
+
+    // our rotation is in fact only 2D, thus quaternion can be simplified
+    double a = transform.at<double>(0, 0);
+    double b = transform.at<double>(1, 0);
+    ros_transform.rotation.w = std::sqrt(2. + 2. * a) * 0.5;
+    ros_transform.rotation.x = 0.;
+    ros_transform.rotation.y = 0.;
+    ros_transform.rotation.z = std::copysign(std::sqrt(2. - 2. * a) * 0.5, b);
+
+    result.push_back(ros_transform);
+  }
+
+  return result;
+}
+
+}  // namespace combine_grids
--- a/map_merge/src/map_merge.cpp
+++ b/map_merge/src/map_merge.cpp
@ -0,0 +1,454 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, Zhi Yan.
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#include <thread>
+
+#include <map_merge/map_merge.h>
+#include <map_merge/ros1_names.hpp>
+#include <rcpputils/asserts.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+
+namespace map_merge
+{
+MapMerge::MapMerge() : Node("map_merge", rclcpp::NodeOptions()
+                                       .allow_undeclared_parameters(true)
+                                       .automatically_declare_parameters_from_overrides(true)),
+subscriptions_size_(0)
+{
+  std::string frame_id;
+  std::string merged_map_topic;
+
+  if (!this->has_parameter("merging_rate")) this->declare_parameter<double>("merging_rate", 4.0);
+  if (!this->has_parameter("discovery_rate")) this->declare_parameter<double>("discovery_rate", 0.05);
+  if (!this->has_parameter("estimation_rate")) this->declare_parameter<double>("estimation_rate", 0.5);
+  if (!this->has_parameter("known_init_poses")) this->declare_parameter<bool>("known_init_poses", true);
+  if (!this->has_parameter("estimation_confidence")) this->declare_parameter<double>("estimation_confidence", 1.0);
+  if (!this->has_parameter("robot_map_topic")) this->declare_parameter<std::string>("robot_map_topic", "map");
+  if (!this->has_parameter("robot_map_updates_topic")) this->declare_parameter<std::string>("robot_map_updates_topic", "map_updates");
+  if (!this->has_parameter("robot_namespace")) this->declare_parameter<std::string>("robot_namespace", "");
+  if (!this->has_parameter("merged_map_topic")) this->declare_parameter<std::string>("merged_map_topic", "map");
+  if (!this->has_parameter("world_frame")) this->declare_parameter<std::string>("world_frame", "world");
+  
+  this->get_parameter("merging_rate", merging_rate_);
+  this->get_parameter("discovery_rate", discovery_rate_);
+  this->get_parameter("estimation_rate", estimation_rate_);
+  this->get_parameter("known_init_poses", have_initial_poses_);
+  this->get_parameter("estimation_confidence", confidence_threshold_);
+  this->get_parameter("robot_map_topic", robot_map_topic_);
+  this->get_parameter("robot_map_updates_topic", robot_map_updates_topic_);
+  this->get_parameter("robot_namespace", robot_namespace_);
+  this->get_parameter("merged_map_topic", merged_map_topic);
+  this->get_parameter("world_frame", world_frame_);
+
+
+  /* publishing */
+  // Create a publisher using the QoS settings to emulate a ROS1 latched topic
+  merged_map_publisher_ =
+      this->create_publisher<nav_msgs::msg::OccupancyGrid>(merged_map_topic, 
+      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+  // Timers
+  map_merging_timer_ = this->create_wall_timer(
+    std::chrono::milliseconds((uint16_t)(1000.0 / merging_rate_)),
+    [this]() { mapMerging(); });
+  // execute right away to simulate the ros1 first while loop on a thread
+  map_merging_timer_->execute_callback();
+
+  topic_subscribing_timer_ = this->create_wall_timer(
+    std::chrono::milliseconds((uint16_t)(1000.0 / discovery_rate_)),
+    [this]() { topicSubscribing(); });
+
+  // For topicSubscribing() we need to spin briefly for the discovery to happen
+  rclcpp::Rate r(100);
+  int i = 0;
+  while (rclcpp::ok() && i < 100) {
+    rclcpp::spin_some(this->get_node_base_interface());
+    r.sleep();
+    i++;
+  }
+  topic_subscribing_timer_->execute_callback(); 
+
+  if (!have_initial_poses_){
+    pose_estimation_timer_ = this->create_wall_timer(
+      std::chrono::milliseconds((uint16_t)(1000.0 / estimation_rate_)),
+      [this]() { poseEstimation(); });
+    // execute right away to simulate the ros1 first while loop on a thread
+    pose_estimation_timer_->execute_callback(); 
+  }
+}
+
+/*
+ * Subcribe to pose and map topics
+ */
+void MapMerge::topicSubscribing()
+{
+  RCLCPP_DEBUG(logger_, "Robot discovery started.");
+  RCLCPP_INFO_ONCE(logger_, "Robot discovery started.");
+
+  // ros::master::V_TopicInfo topic_infos;
+  geometry_msgs::msg::Transform init_pose;
+  std::string robot_name;
+  std::string map_topic;
+  std::string map_updates_topic;
+
+  // ros::master::getTopics(topic_infos);
+  std::map<std::string, std::vector<std::string>> topic_infos = this->get_topic_names_and_types();
+
+  for (const auto& topic_it : topic_infos) {
+    std::string topic_name = topic_it.first;
+    std::vector<std::string> topic_types = topic_it.second;
+    // iterate over all topic types
+    for (const auto& topic_type : topic_types) {
+      // RCLCPP_INFO(logger_, "Topic: %s, type: %s", topic_name.c_str(), topic_type.c_str());
+
+      // we check only map topic
+      if (!isRobotMapTopic(topic_name, topic_type)) {
+        continue;
+      }
+    
+      robot_name = robotNameFromTopic(topic_name);
+      if (robots_.count(robot_name)) {
+        // we already know this robot
+        continue;
+      }
+
+      if (have_initial_poses_ && !getInitPose(robot_name, init_pose)) {
+        RCLCPP_WARN(logger_, "Couldn't get initial position for robot [%s]\n"
+                "did you defined parameters map_merge/init_pose_[xyz]? in robot "
+                "namespace? If you want to run merging without known initial "
+                "positions of robots please set `known_init_poses` parameter "
+                "to false. See relevant documentation for details.",
+                robot_name.c_str());
+        continue;
+      }
+
+      RCLCPP_INFO(logger_, "adding robot [%s] to system", robot_name.c_str());
+      {
+        // We don't lock since because of ROS2 default executor only a callback can run at a time
+        // std::lock_guard<boost::shared_mutex> lock(subscriptions_mutex_);
+        subscriptions_.emplace_front();
+        ++subscriptions_size_;
+      }
+
+      // no locking here. robots_ are used only in this procedure
+      MapSubscription& subscription = subscriptions_.front();
+      robots_.insert({robot_name, &subscription});
+      subscription.initial_pose = init_pose;
+
+      /* subscribe callbacks */
+      map_topic = ros1_names::append(robot_name, robot_map_topic_);
+      map_updates_topic =
+          ros1_names::append(robot_name, robot_map_updates_topic_);
+      RCLCPP_INFO(logger_, "Subscribing to MAP topic: %s.", map_topic.c_str());
+      auto map_qos = rclcpp::QoS(rclcpp::KeepLast(50)).transient_local().reliable();
+      subscription.map_sub = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
+          map_topic, map_qos,
+          [this, &subscription](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
+            fullMapUpdate(msg, subscription);
+          });
+      RCLCPP_INFO(logger_, "Subscribing to MAP updates topic: %s.",
+              map_updates_topic.c_str());
+      subscription.map_updates_sub =
+          this->create_subscription<map_msgs::msg::OccupancyGridUpdate>(
+              map_updates_topic, map_qos,
+              [this, &subscription](
+                  const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg) {
+                partialMapUpdate(msg, subscription);
+              });
+    }
+  }
+}
+
+/*
+ * mapMerging()
+ */
+void MapMerge::mapMerging()
+{
+  RCLCPP_DEBUG(logger_, "Map merging started.");
+  RCLCPP_INFO_ONCE(logger_, "Map merging started.");
+
+  if (have_initial_poses_) {
+    // TODO: attempt fix for SLAM toolbox: add method for padding grids to same size
+
+    std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> grids;
+    std::vector<geometry_msgs::msg::Transform> transforms;
+    grids.reserve(subscriptions_size_);
+    {
+      // We don't lock since because of ROS2 default executor only a callback can run
+      // boost::shared_lock<boost::shared_mutex> lock(subscriptions_mutex_);
+      for (auto& subscription : subscriptions_) {
+        // std::lock_guard<std::mutex> s_lock(subscription.mutex);
+
+        grids.push_back(subscription.readonly_map);
+        transforms.push_back(subscription.initial_pose);
+      }
+    }
+
+
+    // we don't need to lock here, because when have_initial_poses_ is true we
+    // will not run concurrently on the pipeline
+    pipeline_.feed(grids.begin(), grids.end());
+    pipeline_.setTransforms(transforms.begin(), transforms.end());
+  }
+
+  // nav_msgs::OccupancyGridPtr merged_map;
+  nav_msgs::msg::OccupancyGrid::SharedPtr merged_map;
+  {
+    std::lock_guard<std::mutex> lock(pipeline_mutex_);
+    merged_map = pipeline_.composeGrids();
+  }
+  if (!merged_map) {
+    // RCLCPP_INFO(logger_, "No map merged");
+    return;
+  }
+
+  RCLCPP_DEBUG(logger_, "all maps merged, publishing");
+  // RCLCPP_INFO(logger_, "all maps merged, publishing");
+  auto now = this->now();
+  merged_map->info.map_load_time = now;
+  merged_map->header.stamp = now;
+  merged_map->header.frame_id = world_frame_;
+
+  rcpputils::assert_true(merged_map->info.resolution > 0.f);
+  merged_map_publisher_->publish(*merged_map);
+}
+
+void MapMerge::poseEstimation()
+{
+  RCLCPP_DEBUG(logger_, "Grid pose estimation started.");
+  RCLCPP_INFO_ONCE(logger_, "Grid pose estimation started.");
+  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> grids;
+  grids.reserve(subscriptions_size_);
+
+  {
+    // We don't lock since because of ROS2 default executor only a callback can run
+    // boost::shared_lock<boost::shared_mutex> lock(subscriptions_mutex_);
+    for (auto& subscription : subscriptions_) {
+      // std::lock_guard<std::mutex> s_lock(subscription.mutex);
+      grids.push_back(subscription.readonly_map);
+    }
+  }
+
+  // Print grids size
+  // RCLCPP_INFO(logger_, "Grids size: %d", grids.size());
+
+  std::lock_guard<std::mutex> lock(pipeline_mutex_);
+  pipeline_.feed(grids.begin(), grids.end());
+  // TODO allow user to change feature type
+  bool success = pipeline_.estimateTransforms(combine_grids::FeatureType::AKAZE,
+                               confidence_threshold_);
+  // bool success = pipeline_.estimateTransforms(combine_grids::FeatureType::SURF,
+  //                              confidence_threshold_);
+  // bool success = pipeline_.estimateTransforms(combine_grids::FeatureType::ORB,
+  //                              confidence_threshold_);
+  if (!success) {
+    RCLCPP_INFO(logger_, "No grid poses estimated");
+  }
+}
+
+// void MapMerge::fullMapUpdate(const nav_msgs::OccupancyGrid::ConstPtr& msg,
+//                              MapSubscription& subscription)
+void MapMerge::fullMapUpdate(const nav_msgs::msg::OccupancyGrid::SharedPtr msg,
+                     MapSubscription& subscription)
+{
+  RCLCPP_DEBUG(logger_, "received full map update");
+  // RCLCPP_INFO(logger_, "received full map update");
+  std::lock_guard<std::mutex> lock(subscription.mutex);
+  if (subscription.readonly_map){
+    // ros2 header .stamp don't support > operator, we need to create them explicitly
+    auto t1 = rclcpp::Time(subscription.readonly_map->header.stamp);
+    auto t2 = rclcpp::Time(msg->header.stamp);
+    if (t1 > t2) {
+      // we have been overrunned by faster update. our work was useless.
+      return;
+    }
+  }
+
+  subscription.readonly_map = msg;
+  subscription.writable_map = nullptr;
+}
+
+// void MapMerge::partialMapUpdate(
+//     const map_msgs::OccupancyGridUpdate::ConstPtr& msg,
+//     MapSubscription& subscription)
+void MapMerge::partialMapUpdate(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg,
+                        MapSubscription& subscription)
+{
+  RCLCPP_DEBUG(logger_, "received partial map update");
+
+  if (msg->x < 0 || msg->y < 0) {
+    RCLCPP_ERROR(logger_, "negative coordinates, invalid update. x: %d, y: %d", msg->x,
+              msg->y);
+    return;
+  }
+
+  size_t x0 = static_cast<size_t>(msg->x);
+  size_t y0 = static_cast<size_t>(msg->y);
+  size_t xn = msg->width + x0;
+  size_t yn = msg->height + y0;
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr map;
+  nav_msgs::msg::OccupancyGrid::ConstSharedPtr readonly_map;  // local copy
+  {
+    // load maps
+    std::lock_guard<std::mutex> lock(subscription.mutex);
+    map = subscription.writable_map;
+    readonly_map = subscription.readonly_map;
+  }
+
+  if (!readonly_map) {
+    RCLCPP_WARN(logger_, "received partial map update, but don't have any full map to "
+             "update. skipping.");
+    return;
+  }
+
+  // // we don't have partial map to take update, we must copy readonly map and
+  // // update new writable map
+  if (!map) {
+    map.reset(new nav_msgs::msg::OccupancyGrid(*readonly_map));
+  }
+
+  size_t grid_xn = map->info.width;
+  size_t grid_yn = map->info.height;
+
+  if (xn > grid_xn || x0 > grid_xn || yn > grid_yn || y0 > grid_yn) {
+    RCLCPP_WARN(logger_, "received update doesn't fully fit into existing map, "
+             "only part will be copied. received: [%lu, %lu], [%lu, %lu] "
+             "map is: [0, %lu], [0, %lu]",
+             x0, xn, y0, yn, grid_xn, grid_yn);
+  }
+
+  // update map with data
+  size_t i = 0;
+  for (size_t y = y0; y < yn && y < grid_yn; ++y) {
+    for (size_t x = x0; x < xn && x < grid_xn; ++x) {
+      size_t idx = y * grid_xn + x;  // index to grid for this specified cell
+      map->data[idx] = msg->data[i];
+      ++i;
+    }
+  }
+  // update time stamp
+  map->header.stamp = msg->header.stamp;
+
+  {
+    // store back updated map
+    std::lock_guard<std::mutex> lock(subscription.mutex);
+    if (subscription.readonly_map){
+      // ros2 header .stamp don't support > operator, we need to create them explicitly
+      auto t1 = rclcpp::Time(subscription.readonly_map->header.stamp);
+      auto t2 = rclcpp::Time(map->header.stamp);
+      if (t1 > t2) {
+        // we have been overrunned by faster update. our work was useless.
+        return;
+      }
+    }
+    subscription.writable_map = map;
+    subscription.readonly_map = map;
+  }
+}
+
+std::string MapMerge::robotNameFromTopic(const std::string& topic)
+{
+  return ros1_names::parentNamespace(topic);
+}
+
+/* identifies topic via suffix */
+bool MapMerge::isRobotMapTopic(const std::string topic, std::string type)
+{
+  /* test whether topic is robot_map_topic_ */
+  std::string topic_namespace = ros1_names::parentNamespace(topic);
+  bool is_map_topic = ros1_names::append(topic_namespace, robot_map_topic_) == topic;
+
+  // /* test whether topic contains *anywhere* robot namespace */
+  auto pos = topic.find(robot_namespace_);
+  bool contains_robot_namespace = pos != std::string::npos;
+
+  // /* we support only occupancy grids as maps */
+  bool is_occupancy_grid = type == "nav_msgs/msg/OccupancyGrid";
+
+  // /* we don't want to subcribe on published merged map */
+  bool is_our_topic = merged_map_publisher_->get_topic_name() == topic;
+
+  return is_occupancy_grid && !is_our_topic && contains_robot_namespace &&
+         is_map_topic;
+}
+
+/*
+ * Get robot's initial position
+ */
+bool MapMerge::getInitPose(const std::string& name,
+                           geometry_msgs::msg::Transform& pose)
+{
+  std::string merging_namespace = ros1_names::append(name, "map_merge");
+  double yaw = 0.0;
+
+  bool success =
+      this->get_parameter(ros1_names::append(merging_namespace, "init_pose_x"),
+                      pose.translation.x) &&
+      this->get_parameter(ros1_names::append(merging_namespace, "init_pose_y"),
+                      pose.translation.y) &&
+      this->get_parameter(ros1_names::append(merging_namespace, "init_pose_z"),
+                      pose.translation.z) &&
+      this->get_parameter(ros1_names::append(merging_namespace, "init_pose_yaw"),
+                      yaw);
+
+  tf2::Quaternion q;
+  q.setEuler(0., 0., yaw);
+  pose.rotation = toMsg(q);
+
+  return success;
+}
+}  // namespace map_merge
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  // ROS1 code
+  // // this package is still in development -- start wil debugging enabled
+  // if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
+  //                                    ros::console::levels::Debug)) {
+  //   ros::console::notifyLoggerLevelsChanged();
+  // }
+  
+  // ROS2 code
+  auto node = std::make_shared<map_merge::MapMerge>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}
--- a/map_merge/test/test_merging_pipeline.cpp
+++ b/map_merge/test/test_merging_pipeline.cpp
@ -0,0 +1,322 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#include <combine_grids/grid_warper.h>
+#include <gtest/gtest.h>
+#include <ros/console.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <opencv2/core/utility.hpp>
+#include "testing_helpers.h"
+
+#define private public
+#include <combine_grids/merging_pipeline.h>
+
+const std::array<const char*, 2> hector_maps = {
+    "map00.pgm",
+    "map05.pgm",
+};
+
+const std::array<const char*, 2> gmapping_maps = {
+    "2011-08-09-12-22-52.pgm",
+    "2012-01-28-11-12-01.pgm",
+};
+
+constexpr bool verbose_tests = false;
+
+#define EXPECT_VALID_GRID(grid)                                                \
+  ASSERT_TRUE(static_cast<bool>(grid));                                        \
+  EXPECT_TRUE(consistentData(*grid));                                          \
+  EXPECT_GT(grid->info.resolution, 0);                                         \
+  EXPECT_TRUE(isIdentity(grid->info.origin.orientation))
+
+TEST(MergingPipeline, canStich0Grid)
+{
+  std::vector<nav_msgs::OccupancyGridConstPtr> maps;
+  combine_grids::MergingPipeline merger;
+  merger.feed(maps.begin(), maps.end());
+  EXPECT_TRUE(merger.estimateTransforms());
+  EXPECT_EQ(merger.composeGrids(), nullptr);
+  EXPECT_EQ(merger.getTransforms().size(), 0);
+}
+
+TEST(MergingPipeline, canStich1Grid)
+{
+  auto map = loadMap(hector_maps[1]);
+  combine_grids::MergingPipeline merger;
+  merger.feed(&map, &map + 1);
+  merger.estimateTransforms();
+  auto merged_grid = merger.composeGrids();
+
+  EXPECT_VALID_GRID(merged_grid);
+  // don't use EXPECT_EQ, since it prints too much info
+  EXPECT_TRUE(*merged_grid == *map);
+  // check estimated transforms
+  auto transforms = merger.getTransforms();
+  EXPECT_EQ(transforms.size(), 1);
+  EXPECT_TRUE(isIdentity(transforms[0]));
+}
+
+TEST(MergingPipeline, canStich2Grids)
+{
+  auto maps = loadMaps(hector_maps.begin(), hector_maps.end());
+  combine_grids::MergingPipeline merger;
+  merger.feed(maps.begin(), maps.end());
+  merger.estimateTransforms();
+  auto merged_grid = merger.composeGrids();
+
+  EXPECT_VALID_GRID(merged_grid);
+  // grid size should indicate sucessful merge
+  EXPECT_NEAR(2091, merged_grid->info.width, 30);
+  EXPECT_NEAR(2091, merged_grid->info.height, 30);
+
+  if (verbose_tests) {
+    saveMap("test_canStich2Grids.pgm", merged_grid);
+  }
+}
+
+TEST(MergingPipeline, canStichGridsGmapping)
+{
+  auto maps = loadMaps(gmapping_maps.begin(), gmapping_maps.end());
+  combine_grids::MergingPipeline merger;
+  merger.feed(maps.begin(), maps.end());
+  merger.estimateTransforms();
+  auto merged_grid = merger.composeGrids();
+
+  EXPECT_VALID_GRID(merged_grid);
+  // grid size should indicate sucessful merge
+  EXPECT_NEAR(5427, merged_grid->info.width, 30);
+  EXPECT_NEAR(5427, merged_grid->info.height, 30);
+
+  if (verbose_tests) {
+    saveMap("canStichGridsGmapping.pgm", merged_grid);
+  }
+}
+
+TEST(MergingPipeline, estimationAccuracy)
+{
+  // for this test we measure estimation on the same grid artificially
+  // transformed
+  double angle = 0.523599 /* 30 deg in rads*/;
+  // TODO test also translations
+  double tx = 0;
+  double ty = 0;
+  cv::Matx23d transform{std::cos(angle), -std::sin(angle), tx,
+                        std::sin(angle), std::cos(angle),  ty};
+
+  auto map = loadMap(hector_maps[1]);
+  combine_grids::MergingPipeline merger;
+  merger.feed(&map, &map + 1);
+
+  // warp the map with Affine Transform
+  combine_grids::internal::GridWarper warper;
+  cv::Mat warped;
+  auto roi = warper.warp(merger.images_[0], cv::Mat(transform), warped);
+
+  // add warped map
+  // this relies on internal implementation of merging pipeline
+  merger.grids_.emplace_back();
+  merger.images_.push_back(warped);
+
+  merger.estimateTransforms();
+  auto merged_grid = merger.composeGrids();
+
+  EXPECT_VALID_GRID(merged_grid);
+  // transforms
+  auto transforms = merger.getTransforms();
+  EXPECT_EQ(transforms.size(), 2);
+  EXPECT_TRUE(isIdentity(transforms[0]));
+  tf2::Transform t;
+  tf2::fromMsg(transforms[1], t);
+
+  EXPECT_NEAR(angle, t.getRotation().getAngle(), 1e-2);
+  EXPECT_NEAR(tx - roi.tl().x, t.getOrigin().x(), 2);
+  EXPECT_NEAR(ty - roi.tl().y, t.getOrigin().y(), 2);
+}
+
+TEST(MergingPipeline, transformsRoundTrip)
+{
+  auto map = loadMap(hector_maps[0]);
+  combine_grids::MergingPipeline merger;
+  merger.feed(&map, &map + 1);
+  for (size_t i = 0; i < 1000; ++i) {
+    auto in_transform = randomTransform();
+    merger.setTransforms(&in_transform, &in_transform + 1);
+
+    auto out_transforms = merger.getTransforms();
+    ASSERT_EQ(out_transforms.size(), 1);
+    auto out_transform = out_transforms[0];
+    EXPECT_FLOAT_EQ(in_transform.translation.x, out_transform.translation.x);
+    EXPECT_FLOAT_EQ(in_transform.translation.y, out_transform.translation.y);
+    EXPECT_FLOAT_EQ(in_transform.translation.z, out_transform.translation.z);
+    EXPECT_FLOAT_EQ(in_transform.rotation.x, out_transform.rotation.x);
+    EXPECT_FLOAT_EQ(in_transform.rotation.y, out_transform.rotation.y);
+    EXPECT_FLOAT_EQ(in_transform.rotation.z, out_transform.rotation.z);
+    EXPECT_FLOAT_EQ(in_transform.rotation.w, out_transform.rotation.w);
+  }
+}
+
+TEST(MergingPipeline, setTransformsInternal)
+{
+  auto map = loadMap(hector_maps[0]);
+  combine_grids::MergingPipeline merger;
+  merger.feed(&map, &map + 1);
+
+  for (size_t i = 0; i < 1000; ++i) {
+    auto transform = randomTransform();
+    merger.setTransforms(&transform, &transform + 1);
+
+    ASSERT_EQ(merger.transforms_.size(), 1);
+    auto& transform_internal = merger.transforms_[0];
+    // verify that transforms are the same in 2D
+    tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}};
+    cv::Point3d b[2] = {{1., 0., 1.}, {0., 1., 1.}};
+    for (auto j : {0, 1}) {
+      tf2::Transform t;
+      fromMsg(transform, t);
+      auto p1 = t * a[j];
+      cv::Mat p2 = transform_internal * cv::Mat(b[j]);
+      // some precision is naturally lost during conversion, float precision is
+      // still good for us
+      EXPECT_FLOAT_EQ(p1.x(), p2.at<double>(0));
+      EXPECT_FLOAT_EQ(p1.y(), p2.at<double>(1));
+    }
+  }
+}
+
+TEST(MergingPipeline, getTransformsInternal)
+{
+  auto map = loadMap(hector_maps[0]);
+  combine_grids::MergingPipeline merger;
+  merger.feed(&map, &map + 1);
+
+  // set internal transform
+  merger.transforms_.resize(1);
+  for (size_t i = 0; i < 1000; ++i) {
+    cv::Mat transform_internal = randomTransformMatrix();
+    merger.transforms_[0] = transform_internal;
+    auto transforms = merger.getTransforms();
+    ASSERT_EQ(transforms.size(), 1);
+    // output quaternion should be normalized
+    auto& q = transforms[0].rotation;
+    EXPECT_DOUBLE_EQ(1., q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
+
+    // verify that transforms are the same in 2D
+    tf2::Transform transform;
+    fromMsg(transforms[0], transform);
+    tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}};
+    cv::Point3d b[2] = {{1., 0., 1.}, {0., 1., 1.}};
+    for (auto j : {0, 1}) {
+      auto p1 = transform * a[j];
+      cv::Mat p2 = transform_internal * cv::Mat(b[j]);
+      EXPECT_FLOAT_EQ(p1.x(), p2.at<double>(0));
+      EXPECT_FLOAT_EQ(p1.y(), p2.at<double>(1));
+    }
+  }
+}
+
+TEST(MergingPipeline, setEmptyTransforms)
+{
+  constexpr size_t size = 2;
+  std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
+  std::vector<geometry_msgs::Transform> transforms(size);
+  combine_grids::MergingPipeline merger;
+  merger.feed(maps.begin(), maps.end());
+  merger.setTransforms(transforms.begin(), transforms.end());
+  EXPECT_EQ(merger.composeGrids(), nullptr);
+  EXPECT_EQ(merger.getTransforms().size(), size);
+}
+
+/* empty image may end with identity transform. */
+TEST(MergingPipeline, emptyImageWithTransform)
+{
+  constexpr size_t size = 1;
+  std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
+  std::vector<geometry_msgs::Transform> transforms(size);
+  transforms[0].rotation.z = 1;  // set transform to identity
+  combine_grids::MergingPipeline merger;
+  merger.feed(maps.begin(), maps.end());
+  merger.setTransforms(transforms.begin(), transforms.end());
+  EXPECT_EQ(merger.composeGrids(), nullptr);
+  EXPECT_EQ(merger.getTransforms().size(), size);
+}
+
+/* one image may be empty */
+TEST(MergingPipeline, oneEmptyImage)
+{
+  std::vector<nav_msgs::OccupancyGridConstPtr> maps{nullptr,
+                                                    loadMap(gmapping_maps[0])};
+  combine_grids::MergingPipeline merger;
+  merger.feed(maps.begin(), maps.end());
+  merger.estimateTransforms();
+  auto merged_grid = merger.composeGrids();
+  auto transforms = merger.getTransforms();
+
+  EXPECT_VALID_GRID(merged_grid);
+  // don't use EXPECT_EQ, since it prints too much info
+  EXPECT_TRUE(*merged_grid == *maps[1]);
+  // transforms
+  EXPECT_EQ(transforms.size(), 2);
+  EXPECT_TRUE(isIdentity(transforms[1]));
+}
+
+// non-identity known positions etc.
+TEST(MergingPipeline, knownInitPositions)
+{
+  auto maps = loadMaps(gmapping_maps.begin(), gmapping_maps.end());
+  combine_grids::MergingPipeline merger;
+  merger.feed(maps.begin(), maps.end());
+
+  for (size_t i = 0; i < 5; ++i) {
+    std::vector<geometry_msgs::Transform> transforms{randomTransform(),
+                                                     randomTransform()};
+    merger.setTransforms(transforms.begin(), transforms.end());
+    auto merged_grid = merger.composeGrids();
+
+    EXPECT_VALID_GRID(merged_grid);
+  }
+}
+
+int main(int argc, char** argv)
+{
+  ros::Time::init();
+  if (verbose_tests &&
+      ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
+                                     ros::console::levels::Debug)) {
+    ros::console::notifyLoggerLevelsChanged();
+  }
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
--- a/map_merge/test/testing_helpers.h
+++ b/map_merge/test/testing_helpers.h
@ -0,0 +1,158 @@
+#ifndef TESTING_HELPERS_H_
+#define TESTING_HELPERS_H_
+
+#include <nav_msgs/OccupancyGrid.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <opencv2/core/utility.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <random>
+
+const float resolution = 0.05f;
+
+nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename);
+void saveMap(const std::string& filename,
+             const nav_msgs::OccupancyGridConstPtr& map);
+std::tuple<double, double, double> randomAngleTxTy();
+geometry_msgs::Transform randomTransform();
+cv::Mat randomTransformMatrix();
+
+/* map_server is really bad. until there is no replacement I will implement it
+ * by myself */
+template <typename InputIt>
+std::vector<nav_msgs::OccupancyGridConstPtr> loadMaps(InputIt filenames_begin,
+                                                      InputIt filenames_end)
+{
+  std::vector<nav_msgs::OccupancyGridConstPtr> result;
+
+  for (InputIt it = filenames_begin; it != filenames_end; ++it) {
+    result.emplace_back(loadMap(*it));
+  }
+  return result;
+}
+
+nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename)
+{
+  cv::Mat lookUpTable(1, 256, CV_8S);
+  signed char* p = lookUpTable.ptr<signed char>();
+  p[254] = 0;
+  p[205] = -1;
+  p[0] = 100;
+
+  cv::Mat img = cv::imread(filename, cv::IMREAD_GRAYSCALE);
+  if (img.empty()) {
+    throw std::runtime_error("could not load map");
+  }
+  nav_msgs::OccupancyGridPtr grid{new nav_msgs::OccupancyGrid()};
+  grid->info.width = static_cast<uint>(img.size().width);
+  grid->info.height = static_cast<uint>(img.size().height);
+  grid->info.resolution = resolution;
+  grid->data.resize(static_cast<size_t>(img.size().area()));
+  cv::Mat grid_view(img.size(), CV_8S,
+                    const_cast<signed char*>(grid->data.data()));
+  cv::LUT(img, lookUpTable, grid_view);
+
+  return grid;
+}
+
+void saveMap(const std::string& filename,
+             const nav_msgs::OccupancyGridConstPtr& map)
+{
+  cv::Mat lookUpTable(1, 256, CV_8U);
+  uchar* p = lookUpTable.ptr();
+  for (int i = 0; i < 255; ++i) {
+    if (i >= 0 && i < 10)
+      p[i] = 254;
+    else
+      p[i] = 0;
+  }
+  p[255] = 205;
+
+  cv::Mat img(map->info.height, map->info.width, CV_8S,
+              const_cast<signed char*>(map->data.data()));
+  cv::Mat out_img;
+  cv::LUT(img, lookUpTable, out_img);
+  cv::imwrite(filename, out_img);
+}
+
+std::tuple<double, double, double> randomAngleTxTy()
+{
+  static std::mt19937_64 g(156468754 /*magic*/);
+  std::uniform_real_distribution<double> rotation_dis(0., 2 * std::acos(-1));
+  std::uniform_real_distribution<double> translation_dis(-1000, 1000);
+
+  return std::tuple<double, double, double>(rotation_dis(g), translation_dis(g),
+                                            translation_dis(g));
+}
+
+geometry_msgs::Transform randomTransform()
+{
+  double angle, tx, ty;
+  std::tie(angle, tx, ty) = randomAngleTxTy();
+  tf2::Transform transform;
+  tf2::Quaternion rotation;
+  rotation.setEuler(0., 0., angle);
+  rotation.normalize();
+  transform.setRotation(rotation);
+  transform.setOrigin(tf2::Vector3(tx, ty, 0.));
+
+  auto msg = toMsg(transform);
+  // normalize quaternion such that w > 0 (q and -q represents the same
+  // transformation)
+  if (msg.rotation.w < 0.) {
+    msg.rotation.x *= -1.;
+    msg.rotation.y *= -1.;
+    msg.rotation.z *= -1.;
+    msg.rotation.w *= -1.;
+  }
+
+  return msg;
+}
+
+cv::Mat randomTransformMatrix()
+{
+  double angle, tx, ty;
+  std::tie(angle, tx, ty) = randomAngleTxTy();
+  cv::Mat transform =
+      (cv::Mat_<double>(3, 3) << std::cos(angle), -std::sin(angle), tx,
+       std::sin(angle), std::cos(angle), ty, 0., 0., 1.);
+
+  return transform;
+}
+
+static inline bool isIdentity(const geometry_msgs::Transform& transform)
+{
+  tf2::Transform t;
+  tf2::fromMsg(transform, t);
+  return tf2::Transform::getIdentity() == t;
+}
+
+static inline bool isIdentity(const geometry_msgs::Quaternion& rotation)
+{
+  tf2::Quaternion q;
+  tf2::fromMsg(rotation, q);
+  return tf2::Quaternion::getIdentity() == q;
+}
+
+// data size is consistent with height and width
+static inline bool consistentData(const nav_msgs::OccupancyGrid& grid)
+{
+  return grid.info.width * grid.info.height == grid.data.size();
+}
+
+// ignores header, map_load_time and origin
+static inline bool operator==(const nav_msgs::OccupancyGrid& grid1,
+                              const nav_msgs::OccupancyGrid& grid2)
+{
+  bool equal = true;
+  equal &= grid1.info.width == grid2.info.width;
+  equal &= grid1.info.height == grid2.info.height;
+  equal &= std::abs(grid1.info.resolution - grid2.info.resolution) <
+           std::numeric_limits<float>::epsilon();
+  equal &= grid1.data.size() == grid2.data.size();
+  for (size_t i = 0; i < grid1.data.size(); ++i) {
+    equal &= grid1.data[i] == grid2.data[i];
+  }
+  return equal;
+}
+
+#endif  // TESTING_HELPERS_H_