Fix compilation on Jazzy (#37 )

Update params.yaml
Simplify devcontainer base image
2024-07-02 11:20:57 -05:00 · 2023-03-22 16:50:32 -05:00 · 2023-01-09 16:52:47 +00:00 · 2023-01-08 17:32:10 -05:00 · 2023-01-08 03:09:44 -05:00 · 2022-07-21 17:13:43 -05:00
22 changed files with 737 additions and 144 deletions
--- a/.devcontainer/Dockerfile
+++ b/.devcontainer/Dockerfile
@ -0,0 +1,129 @@
+# FROM nvidia/cuda:11.7.1-devel-ubuntu22.04
+FROM ubuntu:jammy
+# FROM nvidia/cuda:11.1.1-cudnn8-devel-ubuntu20.04
+# FROM ubuntu:focal
+
+ENV DEBIAN_FRONTEND noninteractive
+
+RUN apt-get update && apt-get install --no-install-recommends -y \
+    apt-utils \
+    bzip2 \
+    lbzip2 \ 
+    tar \
+    wget \
+    libzbar0 \
+    unzip \
+    build-essential \
+    zlib1g-dev \
+    libcurl4-gnutls-dev \
+    locales \
+    curl  \
+    gnupg2  \
+    lsb-release \
+    ca-certificates \
+    && apt autoremove -y && apt clean -y \
+    && rm -rf /var/lib/apt/lists/*
+
+# https://index.ros.org/doc/ros2/Installation/Crystal/Linux-Install-Debians/
+ENV ROS_DISTRO=humble
+
+ENV LANG=C.UTF-8
+ENV LC_ALL=C.UTF-8
+RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key  -o /usr/share/keyrings/ros-archive-keyring.gpg \
+    && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null
+
+RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' \
+    && wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - \ 
+    && apt update && apt install --no-install-recommends -y \ 
+    ros-${ROS_DISTRO}-ros-base \
+    ros-${ROS_DISTRO}-rclcpp-cascade-lifecycle \
+    ros-${ROS_DISTRO}-geographic-msgs \
+    ros-${ROS_DISTRO}-camera-info-manager \
+    ros-${ROS_DISTRO}-launch-testing-ament-cmake \
+    ros-${ROS_DISTRO}-diagnostic-updater \ 
+    ros-${ROS_DISTRO}-rviz2 \
+    ros-${ROS_DISTRO}-gazebo-ros \
+    ros-${ROS_DISTRO}-gazebo-ros-pkgs \
+    ros-${ROS_DISTRO}-gazebo-msgs \
+    ros-${ROS_DISTRO}-gazebo-plugins \
+    ros-${ROS_DISTRO}-robot-state-publisher \
+    ros-${ROS_DISTRO}-cv-bridge \
+    ros-${ROS_DISTRO}-message-filters \
+    ros-${ROS_DISTRO}-image-transport \ 
+    ros-${ROS_DISTRO}-rqt* \
+    ros-${ROS_DISTRO}-slam-toolbox \
+    ros-${ROS_DISTRO}-navigation2 \
+    ros-${ROS_DISTRO}-nav2-bringup \
+    ros-${ROS_DISTRO}-behaviortree-cpp-v3 \
+    ros-${ROS_DISTRO}-angles \
+    ros-${ROS_DISTRO}-ompl \ 
+    ros-${ROS_DISTRO}-turtlebot3* \
+    ros-${ROS_DISTRO}-image-geometry \
+    && apt autoremove && apt clean -y \
+    && rm -rf /var/lib/apt/lists/* 
+
+# Install ROS2 gazebo dependencies
+RUN apt update && apt-get install --no-install-recommends -y \
+    libglvnd0 \
+    libglx0 \
+    libegl1 \
+    libxext6 \
+    libx11-6 \
+    libblkid-dev \
+    e2fslibs-dev \
+    libboost-all-dev \
+    libaudit-dev \
+    git \
+    nano \
+    # ------------------------------
+    && apt autoremove && apt clean -y \ 
+    && rm -rf /var/lib/apt/lists/*
+
+RUN apt update && apt install --no-install-recommends -y \
+    python3-dev \
+    python3-pip \
+    python3-colcon-common-extensions \
+    && pip3 install rosdep \ 
+    && rosdep init \
+    && rosdep update \
+    && apt autoremove && apt clean -y \
+    && rm -rf /var/lib/apt/lists/* 
+
+# Or your actual UID, GID on Linux if not the default 1000
+ARG USERNAME=dev
+ARG USER_UID=1000
+ARG USER_GID=$USER_UID
+
+# Create a non-root user to use if preferred - see https://aka.ms/vscode-remote/containers/non-root-user.
+RUN groupadd --gid $USER_GID $USERNAME \
+    && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \
+    # Add sudo support for non-root user
+    && apt-get update && apt-get install -y sudo \
+    && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
+    && chmod 0440 /etc/sudoers.d/$USERNAME \
+    && apt-get autoremove && apt-get clean -y \
+    && rm -rf /var/lib/apt/lists/* 
+
+RUN \
+    mkdir -p /home/${USERNAME}/.ignition/fuel/ \
+    && echo "servers:\n  -\n    name: osrf\n    url: https://api.ignitionrobotics.org" >> /home/${USERNAME}/.ignition/fuel/config.yaml \
+    && chown ${USERNAME} /home/${USERNAME}/.ignition \
+    && GAZEBO_SOURCE="source /usr/share/gazebo/setup.sh" \
+    && echo $GAZEBO_SOURCE >> "/home/${USERNAME}/.bashrc" \
+    && chown ${USERNAME} /home/${USERNAME}/.ignition
+
+# ROS2 source setup
+RUN ROS_SOURCE="source /opt/ros/${ROS_DISTRO}/setup.sh" \
+    && echo $ROS_SOURCE >> "/home/${USERNAME}/.bashrc"
+
+WORKDIR /workspace/ros_ws/src
+# Give permission to non-root user to access the workspace
+RUN chown -R ${USERNAME} /workspace/ros_ws
+RUN git clone https://github.com/charlielito/slam_gmapping.git --branch feature/namespace_launch
+
+ENV TURTLEBOT3_MODEL=waffle
+ENV GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models
+
+# Switch back to dialog for any ad-hoc use of apt-get
+ENV DEBIAN_FRONTEND=
+CMD ["/bin/bash"]
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@ -0,0 +1,24 @@
+{
+	"name": "Dev container",
+	"dockerComposeFile": "docker-compose.yml",
+	"service": "m-explore-ros2-humble",
+	"workspaceFolder": "/workspace/ros_ws",
+	"runArgs": [
+		"--runtime=nvidia"
+	],
+	"settings": {
+		"terminal.integrated.automationShell.linux": "/bin/bash",
+	},
+	"extensions": [
+		"ms-azuretools.vscode-docker",
+		"eamodio.gitlens",
+		"streetsidesoftware.code-spell-checker",
+		"oderwat.indent-rainbow",
+		"ms-vsliveshare.vsliveshare",
+		"pkief.material-icon-theme",
+		"ms-python.python",
+		"ms-vscode.cpptools",
+		"xaver.clang-format",
+		"ms-python.vscode-pylance",
+	],
+}
--- a/.devcontainer/docker-compose.yml
+++ b/.devcontainer/docker-compose.yml
@ -0,0 +1,53 @@
+# version: "3.7"
+version: "2.3"
+services:
+  m-explore-ros2-humble:
+    runtime: nvidia
+    build:
+      context: ../
+      dockerfile: .devcontainer/Dockerfile
+    working_dir: /workspace/ros_ws
+    user: dev
+
+    network_mode: host
+    ports:
+      - "80:80"
+    expose:
+      - 80
+
+    init: true
+    privileged: true
+
+    environment:
+      - DISPLAY=$DISPLAY
+      - QT_X11_NO_MITSHM=1
+      - UDEV=1
+      - NVIDIA_DRIVER_CAPABILITIES=compute,utility,display
+    volumes:
+      # Update this to wherever you want VS Code to mount the folder of your project
+      - ..:/workspace/ros_ws/src/m-explore-ros2
+      # Forwards the local Docker socket to the container.
+      - /var/run/docker.sock:/var/run/docker.sock
+      # Enable GUI environments
+      - /tmp/.X11-unix:/tmp/.X11-unix:rw
+
+    devices:
+      - /dev/bus/usb:/dev/bus/usb
+      # NVIDIA drivers to use OpenGL, etc...
+      - /dev/nvidia0:/dev/nvidia0
+      - /dev/nvidiactl:/dev/nvidiactl
+      - /dev/nvidia-uvm:/dev/nvidia-uvm
+      - /dev/input:/dev/input
+      - /dev/snd:/dev/snd
+
+    # Uncomment the next four lines if you will use a ptrace-based debuggers like C++, Go, and Rust.
+    cap_add:
+      - SYS_PTRACE
+    security_opt:
+      - seccomp:unconfined
+
+    # Overrides default command so things don't shut down after the process ends.
+    stdin_open: true
+    tty: true
+
+    command: "/bin/bash"
--- a/.github/workflows/build_test.yaml
+++ b/.github/workflows/build_test.yaml
@ -0,0 +1,47 @@
+name: ROS2 build/tests 
+on:
+  pull_request:
+jobs:
+  build_and_tests:
+    runs-on: ubuntu-20.04
+    strategy:
+      matrix:
+        ros_distribution:
+          - foxy
+          - galactic
+          - humble
+        include:
+          # Foxy Fitzroy (June 2020 - May 2023)
+          - docker_image: ubuntu:focal
+            ros_distribution: foxy
+          # Galactic Geochelone (May 2021 - November 2022)
+          - docker_image: ubuntu:focal
+            ros_distribution: galactic
+          # Humble Hawksbill (May 2022 - May 2027)
+          - docker_image: ubuntu:jammy
+            ros_distribution: humble
+    container:
+      image: ${{ matrix.docker_image }}
+    steps:
+      - name: setup ROS environment
+        uses: ros-tooling/setup-ros@v0.4
+        with:
+          required-ros-distributions: ${{ matrix.ros_distribution }}
+      - name: Checkout repository
+        uses: actions/checkout@v3
+      - name: build multirobot_map_merge and explore_lite
+        uses: ros-tooling/action-ros-ci@v0.2
+        with:
+          package-name: multirobot_map_merge explore_lite
+          target-ros2-distro: ${{ matrix.ros_distribution }}
+          skip-tests: true
+      - name: Run gtests manually multirobot_map_merge
+        run: |
+          . /opt/ros/${{ matrix.ros_distribution }}/setup.sh && . ros_ws/install/setup.sh
+          cd ros_ws/build/multirobot_map_merge
+          ./test_merging_pipeline
+      - name: Run gtests manually explore_lite
+        run: |
+          . /opt/ros/${{ matrix.ros_distribution }}/setup.sh && . ros_ws/install/setup.sh
+          cd ros_ws/build/explore_lite
+          ./test_explore
--- a/README.md
+++ b/README.md
@ -38,7 +38,7 @@ RUNNING
 -------
 To run with a params file just run it with
 ```
-ros2 run explore_lite explore --ros-args --params-file <path_to_ros_ws>/m-explore/explore/config/params.yaml
+ros2 run explore_lite explore --ros-args --params-file <path_to_ros_ws>/m-explore-ros2/explore/config/params.yaml
 ```

 ### Running the explore demo with TB3
@ -49,7 +49,7 @@ 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
+ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True
 ```

 And run this package with
@ -57,7 +57,14 @@ And run this package with
 ros2 launch explore_lite explore.launch.py
 ```

-You can open a rviz2 and add the exploration frontiers marker to see the algorithm working and choose a frontier to explore.
+You can open rviz2 and add the exploration frontiers marker (topic is `explore/frontiers`) to see the algorithm working and the frontier chosen to explore.
+
+### Additional features
+#### Stop/Resume exploration
+By default the exploration node will start right away the frontier-based exploration algorithm. Alternatively, you can stop the exploration by publishing to a `False` to `explore/resume` topic. This will stop the exploration and the robot will stop moving. You can resume the exploration by publishing to `True` to `explore/resume`.
+
+#### Returning to initial pose
+The robot will return to its initial pose after exploration if you want by defining the parameter `return_to_init` to `True` when launching the node.

 #### TB3 troubleshooting (with foxy)
 If you have trouble with TB3 in simulation, as we did, add these extra steps for configuring it.
@ -108,10 +115,12 @@ colcon build --symlink-install --packages-up-to slam_gmapping

 **Note**: You could use [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) instead but you need to use this [experimental branch](https://github.com/robo-friends/m-explore-ros2/tree/feature/slam_toolbox_compat) which is still under development.

-#### Nav2 gazebo spawner
+#### Nav2 gazebo spawner (deprecated in humble)
 To spawn multiple robots, you need the `nav2_gazebo_spawner` which does not come up with the `nav2-bringup` installation. For that, install it with `sudo apt install ros-${ROS_DISTRO}-nav2-gazebo-spawner`.
+Note that was the case for release previous to `humble` but since `humble` release, this package is deprecated and a gazebo node is used for this. So, if you are using `humble` or newer, you don't need to install it.
+
 #### 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).
+This repo has some config examples and launch files for running this package with 2 TB3 robots and a world with nav2. Nonetheless, they are only compatible with the galactic/humble distros and since some breaking changes were introduced in this distro, if you want to try it with another ros2 distro you'll need to tweak those param files for that nav2's distro version (which shouldn't be hard).

 ### 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::
--- a/explore/CMakeLists.txt
+++ b/explore/CMakeLists.txt
@ -88,7 +88,7 @@ target_include_directories(explore PUBLIC
  $<INSTALL_INTERFACE:include>)


-target_link_libraries(explore)
+target_link_libraries(explore ${rclcpp_LIBRARIES})

 ament_target_dependencies(explore ${DEPENDENCIES})

@ -97,6 +97,25 @@ install(TARGETS explore

 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")

+#############
+## Testing ##
+#############
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(test_explore test/test_explore.cpp)
+  target_link_libraries(test_explore ${catkin_LIBRARIES})
+  ament_target_dependencies(test_explore ${DEPENDENCIES})
+
+  
+endif()
+

 ament_export_include_directories(include)
 ament_package()
--- a/explore/config/params.yaml
+++ b/explore/config/params.yaml
@ -1,10 +1,11 @@
 /**:
  ros__parameters:
    robot_base_frame: base_link
+    return_to_init: true
    costmap_topic: map
    costmap_updates_topic: map_updates
    visualize: true
-    planner_frequency: 0.25
+    planner_frequency: 0.15
    progress_timeout: 30.0
    potential_scale: 3.0
    orientation_scale: 0.0
--- a/explore/config/params_costmap.yaml
+++ b/explore/config/params_costmap.yaml
@ -4,7 +4,7 @@ explore_node:
    costmap_topic: /global_costmap/costmap
    costmap_updates_topic: /global_costmap/costmap_updates
    visualize: true
-    planner_frequency: 0.25
+    planner_frequency: 0.15
    progress_timeout: 30.0
    potential_scale: 3.0
    orientation_scale: 0.0
--- a/explore/include/explore/explore.h
+++ b/explore/include/explore/explore.h
@ -49,6 +49,7 @@
 #include <memory>
 #include <mutex>
 #include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/color_rgba.hpp>
 #include <string>
 #include <vector>
@ -79,7 +80,8 @@ public:
  ~Explore();

  void start();
-  void stop();
+  void stop(bool finished_exploring = false);
+  void resume();

  using NavigationGoalHandle =
      rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>;
@ -118,17 +120,26 @@ private:
  rclcpp::TimerBase::SharedPtr exploring_timer_;
  // rclcpp::TimerBase::SharedPtr oneshot_;

+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr resume_subscription_;
+  void resumeCallback(const std_msgs::msg::Bool::SharedPtr msg);
+
  std::vector<geometry_msgs::msg::Point> frontier_blacklist_;
  geometry_msgs::msg::Point prev_goal_;
  double prev_distance_;
  rclcpp::Time last_progress_;
  size_t last_markers_count_;

+  geometry_msgs::msg::Pose initial_pose_;
+  void returnToInitialPose(void);
+
  // parameters
  double planner_frequency_;
  double potential_scale_, orientation_scale_, gain_scale_;
  double progress_timeout_;
  bool visualize_;
+  bool return_to_init_;
+  std::string robot_base_frame_;
+  bool resuming_ = false;
 };
 }  // namespace explore

--- a/explore/src/costmap_client.cpp
+++ b/explore/src/costmap_client.cpp
@ -36,11 +36,11 @@
 *********************************************************************/

 #include <explore/costmap_client.h>
+#include <unistd.h>

 #include <functional>
 #include <mutex>
 #include <string>
-#include <unistd.h>

 namespace explore
 {
--- a/explore/src/explore.cpp
+++ b/explore/src/explore.cpp
@ -40,6 +40,15 @@

 #include <thread>

+inline static bool same_point(const geometry_msgs::msg::Point& one,
+                              const geometry_msgs::msg::Point& two)
+{
+  double dx = one.x - two.x;
+  double dy = one.y - two.y;
+  double dist = sqrt(dx * dx + dy * dy);
+  return dist < 0.01;
+}
+
 namespace explore
 {
 Explore::Explore()
@ -59,6 +68,7 @@ Explore::Explore()
  this->declare_parameter<float>("orientation_scale", 0.0);
  this->declare_parameter<float>("gain_scale", 1.0);
  this->declare_parameter<float>("min_frontier_size", 0.5);
+  this->declare_parameter<bool>("return_to_init", false);

  this->get_parameter("planner_frequency", planner_frequency_);
  this->get_parameter("progress_timeout", timeout);
@ -67,9 +77,13 @@ Explore::Explore()
  this->get_parameter("orientation_scale", orientation_scale_);
  this->get_parameter("gain_scale", gain_scale_);
  this->get_parameter("min_frontier_size", min_frontier_size);
+  this->get_parameter("return_to_init", return_to_init_);
+  this->get_parameter("robot_base_frame", robot_base_frame_);
+
  progress_timeout_ = timeout;
  move_base_client_ =
-      rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(this,ACTION_NAME);
+      rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
+          this, ACTION_NAME);

  search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
                                                 potential_scale_, gain_scale_,
@ -77,18 +91,43 @@ Explore::Explore()

  if (visualize_) {
    marker_array_publisher_ =
-        this->create_publisher<visualization_msgs::msg::MarkerArray>("frontier"
+        this->create_publisher<visualization_msgs::msg::MarkerArray>("explore/"
+                                                                     "frontier"
                                                                     "s",
                                                                     10);
  }

+  // Subscription to resume or stop exploration
+  resume_subscription_ = this->create_subscription<std_msgs::msg::Bool>(
+      "explore/resume", 10,
+      std::bind(&Explore::resumeCallback, this, std::placeholders::_1));
+
  RCLCPP_INFO(logger_, "Waiting to connect to move_base nav2 server");
  move_base_client_->wait_for_action_server();
  RCLCPP_INFO(logger_, "Connected to move_base nav2 server");

+  if (return_to_init_) {
+    RCLCPP_INFO(logger_, "Getting initial pose of the robot");
+    geometry_msgs::msg::TransformStamped transformStamped;
+    std::string map_frame = costmap_client_.getGlobalFrameID();
+    try {
+      transformStamped = tf_buffer_.lookupTransform(
+          map_frame, robot_base_frame_, tf2::TimePointZero);
+      initial_pose_.position.x = transformStamped.transform.translation.x;
+      initial_pose_.position.y = transformStamped.transform.translation.y;
+      initial_pose_.orientation = transformStamped.transform.rotation;
+    } catch (tf2::TransformException& ex) {
+      RCLCPP_ERROR(logger_, "Couldn't find transform from %s to %s: %s",
+                   map_frame.c_str(), robot_base_frame_.c_str(), ex.what());
+      return_to_init_ = false;
+    }
+  }
+
  exploring_timer_ = this->create_wall_timer(
      std::chrono::milliseconds((uint16_t)(1000.0 / planner_frequency_)),
      [this]() { makePlan(); });
+  // Start exploration right away
+  makePlan();
 }

 Explore::~Explore()
@ -96,6 +135,15 @@ Explore::~Explore()
  stop();
 }

+void Explore::resumeCallback(const std_msgs::msg::Bool::SharedPtr msg)
+{
+  if (msg->data) {
+    resume();
+  } else {
+    stop();
+  }
+}
+
 void Explore::visualizeFrontiers(
    const std::vector<frontier_exploration::Frontier>& frontiers)
 {
@ -132,13 +180,14 @@ void Explore::visualizeFrontiers(
  m.color.a = 255;
  // lives forever
 #ifdef ELOQUENT
-  m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning
+  m.lifetime = rclcpp::Duration(0);  // deprecated in galactic warning
 #elif DASHING
-  m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning
+  m.lifetime = rclcpp::Duration(0);  // deprecated in galactic warning
 #else
-  m.lifetime = rclcpp::Duration::from_seconds(0); // foxy onwards
+  m.lifetime = rclcpp::Duration::from_seconds(0);  // foxy onwards
 #endif
-  // m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in galactic
+  // m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in
+  // galactic
  m.frame_locked = true;

  // weighted frontiers are always sorted
@ -149,7 +198,7 @@ void Explore::visualizeFrontiers(
  for (auto& frontier : frontiers) {
    m.type = visualization_msgs::msg::Marker::POINTS;
    m.id = int(id);
-    m.pose.position = {};
+    // m.pose.position = {}; // compile warning
    m.scale.x = 0.1;
    m.scale.y = 0.1;
    m.scale.z = 0.1;
@ -199,7 +248,8 @@ void Explore::makePlan()
  }

  if (frontiers.empty()) {
-    stop();
+    RCLCPP_WARN(logger_, "No frontiers found, stopping.");
+    stop(true);
    return;
  }

@ -215,13 +265,14 @@ void Explore::makePlan()
                         return goalOnBlacklist(f.centroid);
                       });
  if (frontier == frontiers.end()) {
-    stop();
+    RCLCPP_WARN(logger_, "All frontiers traversed/tried out, stopping.");
+    stop(true);
    return;
  }
  geometry_msgs::msg::Point target_position = frontier->centroid;

  // time out if we are not making any progress
-  bool same_goal = prev_goal_ == target_position;
+  bool same_goal = same_point(prev_goal_, target_position);

  prev_goal_ = target_position;
  if (!same_goal || prev_distance_ > frontier->min_distance) {
@ -230,15 +281,19 @@ void Explore::makePlan()
    prev_distance_ = frontier->min_distance;
  }
  // black list if we've made no progress for a long time
-  if (this->now() - last_progress_ >
-      tf2::durationFromSec(progress_timeout_)) {  // TODO: is progress_timeout_
-                                                  // in seconds?
+  if ((this->now() - last_progress_ >
+      tf2::durationFromSec(progress_timeout_)) && !resuming_) {
    frontier_blacklist_.push_back(target_position);
    RCLCPP_DEBUG(logger_, "Adding current goal to black list");
    makePlan();
    return;
  }

+  // ensure only first call of makePlan was set resuming to true
+  if (resuming_) {
+    resuming_ = false;
+  }
+
  // we don't need to do anything if we still pursuing the same goal
  if (same_goal) {
    return;
@ -267,6 +322,20 @@ void Explore::makePlan()
  move_base_client_->async_send_goal(goal, send_goal_options);
 }

+void Explore::returnToInitialPose()
+{
+  RCLCPP_INFO(logger_, "Returning to initial pose.");
+  auto goal = nav2_msgs::action::NavigateToPose::Goal();
+  goal.pose.pose.position = initial_pose_.position;
+  goal.pose.pose.orientation = initial_pose_.orientation;
+  goal.pose.header.frame_id = costmap_client_.getGlobalFrameID();
+  goal.pose.header.stamp = this->now();
+
+  auto send_goal_options =
+      rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions();
+  move_base_client_->async_send_goal(goal, send_goal_options);
+}
+
 bool Explore::goalOnBlacklist(const geometry_msgs::msg::Point& goal)
 {
  constexpr static size_t tolerace = 5;
@ -295,13 +364,16 @@ void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result,
      RCLCPP_DEBUG(logger_, "Goal was aborted");
      frontier_blacklist_.push_back(frontier_goal);
      RCLCPP_DEBUG(logger_, "Adding current goal to black list");
+      // If it was aborted probably because we've found another frontier goal,
+      // so just return and don't make plan again
      return;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_DEBUG(logger_, "Goal was canceled");
+      // If goal canceled might be because exploration stopped from topic. Don't make new plan.
      return;
    default:
      RCLCPP_WARN(logger_, "Unknown result code from move base nav2");
-      return;
+      break;
  }
  // find new goal immediately regardless of planning frequency.
  // execute via timer to prevent dead lock in move_base_client (this is
@ -311,9 +383,9 @@ void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result,
  //     ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); },
  //     true);

-  // TODO: Implement this with ros2 timers?
-  // Because of the async nature of ros2 calls I think this is not needed.
-  // makePlan();
+  // Because of the 1-thread-executor nature of ros2 I think timer is not
+  // needed.
+  makePlan();
 }

 void Explore::start()
@ -321,11 +393,25 @@ void Explore::start()
  RCLCPP_INFO(logger_, "Exploration started.");
 }

-void Explore::stop()
+void Explore::stop(bool finished_exploring)
 {
+  RCLCPP_INFO(logger_, "Exploration stopped.");
  move_base_client_->async_cancel_all_goals();
  exploring_timer_->cancel();
-  RCLCPP_INFO(logger_, "Exploration stopped.");
+
+  if (return_to_init_ && finished_exploring) {
+    returnToInitialPose();
+  }
+}
+
+void Explore::resume()
+{
+  resuming_ = true;
+  RCLCPP_INFO(logger_, "Exploration resuming.");
+  // Reactivate the timer
+  exploring_timer_->reset();
+  // Resume immediately
+  makePlan();
 }

 }  // namespace explore
--- a/explore/test/test_explore.cpp
+++ b/explore/test/test_explore.cpp
@ -0,0 +1,77 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, 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 Carlos Alvarez 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 <gtest/gtest.h>
+#include <geometry_msgs/msg/point.hpp>
+#include <cmath>
+
+#define private public
+
+inline static bool same_point(const geometry_msgs::msg::Point& one,
+                              const geometry_msgs::msg::Point& two)
+{
+  double dx = one.x - two.x;
+  double dy = one.y - two.y;
+  double dist = sqrt(dx * dx + dy * dy);
+  return dist < 0.01;
+}
+
+TEST(Explore, testSameGoal)
+{
+    geometry_msgs::msg::Point goal1;
+    geometry_msgs::msg::Point goal2;
+    // Populate the goal with known values
+    goal1.x = 1.0;
+    goal1.y = 2.0;
+    goal1.z = 3.0;
+
+    goal2.x = 0.0;
+    goal2.y = 0.0;
+    goal2.z = 0.0;
+    auto same_goal = same_point(goal1, goal2);
+    EXPECT_FALSE(same_goal);
+    goal2.x = goal1.x;
+    goal2.y = goal1.y;
+    goal2.z = goal1.z;
+    same_goal = same_point(goal1, goal2);
+    EXPECT_TRUE(same_goal);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
--- a/map_merge/CMakeLists.txt
+++ b/map_merge/CMakeLists.txt
@ -23,6 +23,8 @@ find_package(image_geometry REQUIRED)
 find_package(map_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)

 find_package(Boost REQUIRED COMPONENTS thread)

@ -43,6 +45,8 @@ set(DEPENDENCIES
  map_msgs
  nav_msgs
  tf2_geometry_msgs
+  tf2
+  tf2_ros
  OpenCV
 )

@ -95,6 +99,7 @@ target_link_libraries(map_merge combine_grids)
 # target_link_libraries(map_merge)

 ament_target_dependencies(map_merge ${DEPENDENCIES})
+ament_target_dependencies(combine_grids ${DEPENDENCIES})

 install(
  TARGETS combine_grids
@ -110,35 +115,50 @@ install(TARGETS map_merge

 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)
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()

-#   # 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)
+  find_package(ament_cmake_gtest REQUIRED)

-#   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()
+  # download test data: TODO: ROS2 alternative? For now you'll need to download them manually
+  set(base_url https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge)
+  # ament_download(${base_url}/hector_maps/map00.pgm
+  #   MD5 915609a85793ec1375f310d44f2daf87
+  #   FILENAME ${PROJECT_NAME}_map00.pgm
+  # )
+  execute_process(
+    COMMAND bash download.sh ${CMAKE_BINARY_DIR}/map00.pgm ${base_url}/hector_maps/map00.pgm 915609a85793ec1375f310d44f2daf87
+    WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test
+  )
+  execute_process(
+    COMMAND bash download.sh ${CMAKE_BINARY_DIR}/map05.pgm ${base_url}/hector_maps/map05.pgm cb9154c9fa3d97e5e992592daca9853a
+    WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test
+  )
+  execute_process(
+    COMMAND bash download.sh ${CMAKE_BINARY_DIR}/2011-08-09-12-22-52.pgm ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm 3c2c38e7dec2b7a67f41069ab58badaa
+    WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test
+  )
+  execute_process(
+    COMMAND bash download.sh ${CMAKE_BINARY_DIR}/2012-01-28-11-12-01.pgm ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm 681e704044889c95e47b0c3aadd81f1e
+    WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test
+  )

+  ament_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})
+  
+endif()


+ament_export_include_directories(include)
+ament_export_libraries(combine_grids)
+ament_package()
--- a/map_merge/config/params.yaml
+++ b/map_merge/config/params.yaml
@ -5,7 +5,7 @@ map_merge:
    estimation_rate: 0.5
    estimation_confidence: 0.6
    robot_map_topic: map
-    robot_map_updates_topic: map_udpates
+    robot_map_updates_topic: map_updates
    robot_namespace: ""
    merged_map_topic: map
    world_frame: world
@ -21,4 +21,4 @@ map_merge:
    /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
+    /robot2/map_merge/init_pose_yaw: 0.0
--- a/map_merge/launch/tb3_simulation/bringup_launch.py
+++ b/map_merge/launch/tb3_simulation/bringup_launch.py
@ -163,7 +163,7 @@ def generate_launch_description():
            ),
        ]
    )
-    # Not in GroupAction because namespace were prepended twice because 
+    # Not in GroupAction because namespace were prepended twice because
    # slam_gmapping.launch.py already accepts a namespace argument
    slam_gmapping_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
--- a/map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py
+++ b/map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py
@ -23,7 +23,6 @@ The robots co-exist on a shared environment and are controlled by independent na
 import os

 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription, condition
 from launch.actions import (
    DeclareLaunchArgument,
@ -35,6 +34,7 @@ from launch.actions import (
 from launch.conditions import IfCondition, UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch_ros.actions import Node


 def generate_launch_description():
@ -51,7 +51,7 @@ def generate_launch_description():
        {"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
+    # Names and poses of the robots for unknown poses demo, the must be very close at beginning
    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},
@ -159,39 +159,105 @@ def generate_launch_description():
        output="screen",
    )

+    robot_sdf = LaunchConfiguration("robot_sdf")
+    declare_robot_sdf_cmd = DeclareLaunchArgument(
+        "robot_sdf",
+        default_value=os.path.join(bringup_dir, "worlds", "waffle.model"),
+        description="Full path to robot sdf file to spawn the robot in gazebo",
+    )
+
    # 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),
+        # after humble release, use spawn_entity.py
+        if os.getenv("ROS_DISTRO") == "humble":
+            spawn_robots_cmds.append(
+                Node(
+                    package="gazebo_ros",
+                    executable="spawn_entity.py",
+                    output="screen",
+                    arguments=[
+                        "-entity",
+                        robot_known["name"],
+                        "-file",
+                        robot_sdf,
+                        "-robot_namespace",
+                        TextSubstitution(text=str(robot_known["name"])),
+                        "-x",
+                        TextSubstitution(text=str(robot_known["x_pose"])),
+                        "-y",
+                        TextSubstitution(text=str(robot_known["y_pose"])),
+                        "-z",
+                        TextSubstitution(text=str(robot_known["z_pose"])),
+                        "-R",
+                        "0.0",
+                        "-P",
+                        "0.0",
+                        "-Y",
+                        "0.0",
+                    ],
+                    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),
+            spawn_robots_cmds.append(
+                Node(
+                    package="gazebo_ros",
+                    executable="spawn_entity.py",
+                    output="screen",
+                    arguments=[
+                        "-entity",
+                        robot_unknown["name"],
+                        "-file",
+                        robot_sdf,
+                        "-robot_namespace",
+                        TextSubstitution(text=str(robot_unknown["name"])),
+                        "-x",
+                        TextSubstitution(text=str(robot_unknown["x_pose"])),
+                        "-y",
+                        TextSubstitution(text=str(robot_unknown["y_pose"])),
+                        "-z",
+                        TextSubstitution(text=str(robot_unknown["z_pose"])),
+                        "-R",
+                        "0.0",
+                        "-P",
+                        "0.0",
+                        "-Y",
+                        "0.0",
+                    ],
+                    condition=UnlessCondition(known_init_poses),
+                )
+            )
+        else:
+            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 = []
@ -280,6 +346,7 @@ def generate_launch_description():
    ld.add_action(declare_slam_toolbox_cmd)
    ld.add_action(declare_slam_gmapping_cmd)
    ld.add_action(declare_known_init_poses_cmd)
+    ld.add_action(declare_robot_sdf_cmd)

    # Add the actions to start gazebo, robots and simulations
    ld.add_action(start_gazebo_cmd)
--- a/map_merge/package.xml
+++ b/map_merge/package.xml
@ -20,6 +20,8 @@
  <depend>nav_msgs</depend>
  <depend>map_msgs</depend>
  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
  <!-- used to get OpenCV -->
  <depend>image_geometry</depend>

--- a/map_merge/src/combine_grids/merging_pipeline.cpp
+++ b/map_merge/src/combine_grids/merging_pipeline.cpp
@ -101,28 +101,27 @@ bool MergingPipeline::estimateTransforms(FeatureType feature_type,
  // 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());
+    transforms_.clear();
+    transforms_.resize(images_.size());
+
+    // Making some tests to see if it is better to just return false if no match is found
+    // and not clear the last good transforms found
+    // if (images_.size() != transforms_.size()) {
+    //   transforms_.clear();
+    //   transforms_.resize(images_.size());
+    // }
+    // return false;
+
+    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;
+      }
    }
-    return false;
+    // RCLCPP_INFO(logger, "No match found between maps, setting first non-empty grid as reference frame");
+    return true;
  }

  // // Experimental: should we keep only the best confidence match overall?
--- a/map_merge/test/download.sh
+++ b/map_merge/test/download.sh
@ -0,0 +1,21 @@
+file_name=$1
+url=$2
+md5=$3
+
+# Download the file if it doesn't exist
+if [ ! -f $file_name ]; then
+    wget $url -O $file_name
+fi
+
+# Check the MD5 sum of the file
+echo "Checking MD5 sum of $file_name"
+md5sum -c <<<"$md5 $file_name"
+if [ $? -ne 0 ]; then
+    echo "MD5 sum of $file_name does not match. Downloading it again"
+    wget $url -O $file_name
+    md5sum -c <<<"$md5 $file_name"
+    if [ $? -ne 0 ]; then
+        echo "MD5 sum of $file_name still does not match. Aborting."
+        exit 1
+    fi
+fi
--- a/map_merge/test/download_data.sh
+++ b/map_merge/test/download_data.sh
@ -0,0 +1,5 @@
+base_url=https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge
+wget ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm -P build/multirobot_map_merge
+wget ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm -P build/multirobot_map_merge
+wget ${base_url}/hector_maps/map05.pgm -P build/multirobot_map_merge
+wget ${base_url}/hector_maps/map00.pgm -P build/multirobot_map_merge
--- a/map_merge/test/test_merging_pipeline.cpp
+++ b/map_merge/test/test_merging_pipeline.cpp
@ -3,6 +3,7 @@
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2022, Carlos Alvarez.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
@ -36,8 +37,7 @@

 #include <combine_grids/grid_warper.h>
 #include <gtest/gtest.h>
-#include <ros/console.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+// #include <ros/console.h>
 #include <opencv2/core/utility.hpp>
 #include "testing_helpers.h"

@ -64,12 +64,12 @@ constexpr bool verbose_tests = false;

 TEST(MergingPipeline, canStich0Grid)
 {
-  std::vector<nav_msgs::OccupancyGridConstPtr> maps;
+  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> 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);
+  EXPECT_EQ(merger.getTransforms().size(), (long unsigned int) 0);
 }

 TEST(MergingPipeline, canStich1Grid)
@ -82,10 +82,11 @@ TEST(MergingPipeline, canStich1Grid)

  EXPECT_VALID_GRID(merged_grid);
  // don't use EXPECT_EQ, since it prints too much info
-  EXPECT_TRUE(*merged_grid == *map);
+  EXPECT_TRUE(maps_equal(*merged_grid, *map));
+
  // check estimated transforms
  auto transforms = merger.getTransforms();
-  EXPECT_EQ(transforms.size(), 1);
+  EXPECT_EQ(transforms.size(), (long unsigned int) 1);
  EXPECT_TRUE(isIdentity(transforms[0]));
 }

@ -156,7 +157,7 @@ TEST(MergingPipeline, estimationAccuracy)
  EXPECT_VALID_GRID(merged_grid);
  // transforms
  auto transforms = merger.getTransforms();
-  EXPECT_EQ(transforms.size(), 2);
+  EXPECT_EQ(transforms.size(), (long unsigned int) 2);
  EXPECT_TRUE(isIdentity(transforms[0]));
  tf2::Transform t;
  tf2::fromMsg(transforms[1], t);
@ -176,7 +177,7 @@ TEST(MergingPipeline, transformsRoundTrip)
    merger.setTransforms(&in_transform, &in_transform + 1);

    auto out_transforms = merger.getTransforms();
-    ASSERT_EQ(out_transforms.size(), 1);
+    ASSERT_EQ(out_transforms.size(), (long unsigned int) 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);
@ -198,7 +199,7 @@ TEST(MergingPipeline, setTransformsInternal)
    auto transform = randomTransform();
    merger.setTransforms(&transform, &transform + 1);

-    ASSERT_EQ(merger.transforms_.size(), 1);
+    ASSERT_EQ(merger.transforms_.size(), (long unsigned int) 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.}};
@ -228,7 +229,7 @@ TEST(MergingPipeline, getTransformsInternal)
    cv::Mat transform_internal = randomTransformMatrix();
    merger.transforms_[0] = transform_internal;
    auto transforms = merger.getTransforms();
-    ASSERT_EQ(transforms.size(), 1);
+    ASSERT_EQ(transforms.size(), (long unsigned int) 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);
@ -250,8 +251,8 @@ TEST(MergingPipeline, getTransformsInternal)
 TEST(MergingPipeline, setEmptyTransforms)
 {
  constexpr size_t size = 2;
-  std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
-  std::vector<geometry_msgs::Transform> transforms(size);
+  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps(size);
+  std::vector<geometry_msgs::msg::Transform> transforms(size);
  combine_grids::MergingPipeline merger;
  merger.feed(maps.begin(), maps.end());
  merger.setTransforms(transforms.begin(), transforms.end());
@ -263,8 +264,8 @@ TEST(MergingPipeline, setEmptyTransforms)
 TEST(MergingPipeline, emptyImageWithTransform)
 {
  constexpr size_t size = 1;
-  std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
-  std::vector<geometry_msgs::Transform> transforms(size);
+  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps(size);
+  std::vector<geometry_msgs::msg::Transform> transforms(size);
  transforms[0].rotation.z = 1;  // set transform to identity
  combine_grids::MergingPipeline merger;
  merger.feed(maps.begin(), maps.end());
@ -276,7 +277,7 @@ TEST(MergingPipeline, emptyImageWithTransform)
 /* one image may be empty */
 TEST(MergingPipeline, oneEmptyImage)
 {
-  std::vector<nav_msgs::OccupancyGridConstPtr> maps{nullptr,
+  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps{nullptr,
                                                    loadMap(gmapping_maps[0])};
  combine_grids::MergingPipeline merger;
  merger.feed(maps.begin(), maps.end());
@ -286,9 +287,9 @@ TEST(MergingPipeline, oneEmptyImage)

  EXPECT_VALID_GRID(merged_grid);
  // don't use EXPECT_EQ, since it prints too much info
-  EXPECT_TRUE(*merged_grid == *maps[1]);
+  EXPECT_TRUE(maps_equal(*merged_grid, *maps[1]));
  // transforms
-  EXPECT_EQ(transforms.size(), 2);
+  EXPECT_EQ(transforms.size(), (long unsigned int) 2);
  EXPECT_TRUE(isIdentity(transforms[1]));
 }

@ -300,7 +301,7 @@ TEST(MergingPipeline, knownInitPositions)
  merger.feed(maps.begin(), maps.end());

  for (size_t i = 0; i < 5; ++i) {
-    std::vector<geometry_msgs::Transform> transforms{randomTransform(),
+    std::vector<geometry_msgs::msg::Transform> transforms{randomTransform(),
                                                     randomTransform()};
    merger.setTransforms(transforms.begin(), transforms.end());
    auto merged_grid = merger.composeGrids();
@ -311,12 +312,12 @@ TEST(MergingPipeline, knownInitPositions)

 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();
-  }
+  // 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
@ -1,28 +1,33 @@
 #ifndef TESTING_HELPERS_H_
 #define TESTING_HELPERS_H_

-#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <geometry_msgs/msg/transform.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/convert.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);
+nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename);
 void saveMap(const std::string& filename,
-             const nav_msgs::OccupancyGridConstPtr& map);
+             const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map);
 std::tuple<double, double, double> randomAngleTxTy();
-geometry_msgs::Transform randomTransform();
+geometry_msgs::msg::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,
+std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> loadMaps(InputIt filenames_begin,
                                                      InputIt filenames_end)
 {
-  std::vector<nav_msgs::OccupancyGridConstPtr> result;
+  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> result;

  for (InputIt it = filenames_begin; it != filenames_end; ++it) {
    result.emplace_back(loadMap(*it));
@ -30,7 +35,7 @@ std::vector<nav_msgs::OccupancyGridConstPtr> loadMaps(InputIt filenames_begin,
  return result;
 }

-nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename)
+nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename)
 {
  cv::Mat lookUpTable(1, 256, CV_8S);
  signed char* p = lookUpTable.ptr<signed char>();
@ -42,7 +47,7 @@ nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename)
  if (img.empty()) {
    throw std::runtime_error("could not load map");
  }
-  nav_msgs::OccupancyGridPtr grid{new nav_msgs::OccupancyGrid()};
+  nav_msgs::msg::OccupancyGrid::SharedPtr grid{new nav_msgs::msg::OccupancyGrid()};
  grid->info.width = static_cast<uint>(img.size().width);
  grid->info.height = static_cast<uint>(img.size().height);
  grid->info.resolution = resolution;
@ -55,7 +60,7 @@ nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename)
 }

 void saveMap(const std::string& filename,
-             const nav_msgs::OccupancyGridConstPtr& map)
+             const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map)
 {
  cv::Mat lookUpTable(1, 256, CV_8U);
  uchar* p = lookUpTable.ptr();
@ -84,7 +89,7 @@ std::tuple<double, double, double> randomAngleTxTy()
                                            translation_dis(g));
 }

-geometry_msgs::Transform randomTransform()
+geometry_msgs::msg::Transform randomTransform()
 {
  double angle, tx, ty;
  std::tie(angle, tx, ty) = randomAngleTxTy();
@ -119,14 +124,14 @@ cv::Mat randomTransformMatrix()
  return transform;
 }

-static inline bool isIdentity(const geometry_msgs::Transform& transform)
+static inline bool isIdentity(const geometry_msgs::msg::Transform& transform)
 {
  tf2::Transform t;
  tf2::fromMsg(transform, t);
  return tf2::Transform::getIdentity() == t;
 }

-static inline bool isIdentity(const geometry_msgs::Quaternion& rotation)
+static inline bool isIdentity(const geometry_msgs::msg::Quaternion& rotation)
 {
  tf2::Quaternion q;
  tf2::fromMsg(rotation, q);
@ -134,15 +139,32 @@ static inline bool isIdentity(const geometry_msgs::Quaternion& rotation)
 }

 // data size is consistent with height and width
-static inline bool consistentData(const nav_msgs::OccupancyGrid& grid)
+static inline bool consistentData(const nav_msgs::msg::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)
+// static inline bool operator==(const nav_msgs::msg::OccupancyGrid::SharedPtr grid1,
+//                               const nav_msgs::msg::OccupancyGrid::SharedPtr 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;
+// }
+
+// ignores header, map_load_time and origin
+static inline bool maps_equal(const nav_msgs::msg::OccupancyGrid& grid1,
+                              const nav_msgs::msg::OccupancyGrid& grid2)
 {
+  // std::cout << "asdasdadsdth: " << std::endl;
  bool equal = true;
  equal &= grid1.info.width == grid2.info.width;
  equal &= grid1.info.height == grid2.info.height;
Author	SHA1	Message	Date
Tim Clephas	9c121aebe5	Fix compilation on Jazzy (#37 )	2024-07-02 11:20:57 -05:00
Carlos Andrés Álvarez Restrepo	e40e857b6a	Update params.yaml	2023-03-22 16:50:32 -05:00
charlielito	997fad5585	Simplify devcontainer base image	2023-01-09 16:52:47 +00:00
Carlos Andrés Álvarez Restrepo	dca4400a9f	Fix humble multirobot launch (#30 ) * fix multirobot simulation humble * Add devcotainer for development * Add user non root * Finish dockerfile working with humble * Fix launch for humble * Clarify change in humble	2023-01-08 17:32:10 -05:00
Carlos Andrés Álvarez Restrepo	41eff876ee	Fix humble and add action to test all ros distros (#29 ) * add action to compile all distros * fix ci * fix humble * update ci * update ci * test ci * test ci * test ci * test ci * test ci * test ci * test ci * test ci * test ci	2023-01-08 03:09:44 -05:00
Carlos Andrés Álvarez Restrepo	d75bb078ae	Update README.md	2022-07-21 17:13:43 -05:00
Carlos Andrés Álvarez Restrepo	5582e7dd81	Fix bug when resuming/stopping the exploration (#23 ) * Fix bug when resuming/stopping the exploration and add toy test * Add to actions explore_lite test	2022-07-04 19:07:52 -05:00
Carlos Andrés Álvarez Restrepo	159004e0c9	Port tests: (#21 ) * MVP compiles * All tests passing * Add downloading maps with bash scrcipt in test map merigng * Fix tests * Remove launch file testing in ros2 * Add action * test * fox workflow file * Test * Skip tests * Run tests * Fix it * Chnage to fail tests * Fix tests * Try to make fail tests * Fix tests	2022-06-29 10:48:13 -05:00
Carlos Andrés Álvarez Restrepo	5dfdb3a68e	Feature/return initial pose (#20 ) * Add return to initial pose param and logic * Format clang * Add readme feature * Fix log * Fix return statement * Deelete commented block	2022-06-09 13:18:58 -05:00
Carlos Andrés Álvarez Restrepo	cc755cacac	Feature:start stop (#18 ) * Add topic for stop/resume exploration + docs * Clang format	2022-06-09 12:23:58 -05:00