.devcontainer/Dockerfile

@@ -1,129 +0,0 @@
-# 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"]

.devcontainer/devcontainer.json

@@ -1,24 +0,0 @@
-{
-	"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",
-	],
-}

.devcontainer/docker-compose.yml

@@ -1,53 +0,0 @@
-# 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"

.github/workflows/build_test.yaml

@@ -1,47 +0,0 @@
-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

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-ros2/explore/config/params.yaml
+ros2 run explore_lite explore --ros-args --params-file <path_to_ros_ws>/m-explore/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,14 +57,7 @@ And run this package with
 ros2 launch explore_lite explore.launch.py
 ```
 
-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.
+You can open a rviz2 and add the exploration frontiers marker to see the algorithm working and choose a frontier 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.
@@ -115,12 +108,10 @@ 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 (deprecated in humble)
+#### Nav2 gazebo spawner
 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/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).
+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::

explore/CMakeLists.txt

@@ -88,7 +88,7 @@ target_include_directories(explore PUBLIC
   $<INSTALL_INTERFACE:include>)
 
 
-target_link_libraries(explore ${rclcpp_LIBRARIES})
+target_link_libraries(explore)
 
 ament_target_dependencies(explore ${DEPENDENCIES})
 
@@ -97,25 +97,6 @@ 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()

explore/config/params.yaml

@@ -1,11 +1,10 @@
 /**:
   ros__parameters:
     robot_base_frame: base_link
-    return_to_init: true
     costmap_topic: map
     costmap_updates_topic: map_updates
     visualize: true
-    planner_frequency: 0.15
+    planner_frequency: 0.25
     progress_timeout: 30.0
     potential_scale: 3.0
     orientation_scale: 0.0

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.15
+    planner_frequency: 0.25
     progress_timeout: 30.0
     potential_scale: 3.0
     orientation_scale: 0.0

explore/include/explore/explore.h

@@ -49,7 +49,6 @@
 #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>
@@ -80,8 +79,7 @@ public:
   ~Explore();
 
   void start();
-  void stop(bool finished_exploring = false);
+  void stop();
-  void resume();
 
   using NavigationGoalHandle =
       rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>;
@@ -120,26 +118,17 @@ 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
 

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
 {

explore/src/explore.cpp

@@ -40,15 +40,6 @@
 
 #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()
@@ -68,7 +59,6 @@ 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);
@@ -77,13 +67,9 @@ 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>(
+      rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(this,ACTION_NAME);
-          this, ACTION_NAME);
 
   search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
                                                  potential_scale_, gain_scale_,
@@ -91,43 +77,18 @@ Explore::Explore()
 
   if (visualize_) {
     marker_array_publisher_ =
-        this->create_publisher<visualization_msgs::msg::MarkerArray>("explore/"
+        this->create_publisher<visualization_msgs::msg::MarkerArray>("frontier"
-                                                                     "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()
@@ -135,15 +96,6 @@ 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)
 {
@@ -180,14 +132,13 @@ 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
+  // m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in galactic
-  // galactic
   m.frame_locked = true;
 
   // weighted frontiers are always sorted
@@ -198,7 +149,7 @@ void Explore::visualizeFrontiers(
   for (auto& frontier : frontiers) {
     m.type = visualization_msgs::msg::Marker::POINTS;
     m.id = int(id);
-    // m.pose.position = {}; // compile warning
+    m.pose.position = {};
     m.scale.x = 0.1;
     m.scale.y = 0.1;
     m.scale.z = 0.1;
@@ -248,8 +199,7 @@ void Explore::makePlan()
   }
 
   if (frontiers.empty()) {
-    RCLCPP_WARN(logger_, "No frontiers found, stopping.");
+    stop();
-    stop(true);
     return;
   }
 
@@ -265,14 +215,13 @@ void Explore::makePlan()
                          return goalOnBlacklist(f.centroid);
                        });
   if (frontier == frontiers.end()) {
-    RCLCPP_WARN(logger_, "All frontiers traversed/tried out, stopping.");
+    stop();
-    stop(true);
     return;
   }
   geometry_msgs::msg::Point target_position = frontier->centroid;
 
   // time out if we are not making any progress
-  bool same_goal = same_point(prev_goal_, target_position);
+  bool same_goal = prev_goal_ == target_position;
 
   prev_goal_ = target_position;
   if (!same_goal || prev_distance_ > frontier->min_distance) {
@@ -281,19 +230,15 @@ 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_ >
+  if (this->now() - last_progress_ >
-      tf2::durationFromSec(progress_timeout_)) && !resuming_) {
+      tf2::durationFromSec(progress_timeout_)) {  // TODO: is progress_timeout_
+                                                  // in seconds?
     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;
@@ -322,20 +267,6 @@ 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;
@@ -364,16 +295,13 @@ 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");
-      break;
+      return;
   }
   // find new goal immediately regardless of planning frequency.
   // execute via timer to prevent dead lock in move_base_client (this is
@@ -383,9 +311,9 @@ void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result,
   //     ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); },
   //     true);
 
-  // Because of the 1-thread-executor nature of ros2 I think timer is not
+  // TODO: Implement this with ros2 timers?
-  // needed.
+  // Because of the async nature of ros2 calls I think this is not needed.
-  makePlan();
+  // makePlan();
 }
 
 void Explore::start()
@@ -393,25 +321,11 @@ void Explore::start()
   RCLCPP_INFO(logger_, "Exploration started.");
 }
 
-void Explore::stop(bool finished_exploring)
+void Explore::stop()
 {
-  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

explore/test/test_explore.cpp

@@ -1,77 +0,0 @@
-/*********************************************************************
- *
- * 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();
-}

map_merge/CMakeLists.txt

@@ -23,8 +23,6 @@ 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)
 
@@ -45,8 +43,6 @@ set(DEPENDENCIES
   map_msgs
   nav_msgs
   tf2_geometry_msgs
-  tf2
-  tf2_ros
   OpenCV
 )
 
@@ -99,7 +95,6 @@ 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
@@ -115,50 +110,35 @@ 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(BUILD_TESTING)
+# if(CATKIN_ENABLE_TESTING)
-  find_package(ament_lint_auto REQUIRED)
+#   find_package(roslaunch 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)
+#   # 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)
 
-  # download test data: TODO: ROS2 alternative? For now you'll need to download them manually
+#   catkin_add_gtest(test_merging_pipeline test/test_merging_pipeline.cpp)
-  set(base_url https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge)
+#   # ensure that test data are downloaded before we run tests
-  # ament_download(${base_url}/hector_maps/map00.pgm
+#   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)
-  #   MD5 915609a85793ec1375f310d44f2daf87
+#   target_link_libraries(test_merging_pipeline combine_grids ${catkin_LIBRARIES})
-  #   FILENAME ${PROJECT_NAME}_map00.pgm
+
-  # )
+#   # test all launch files
-  execute_process(
+#   # do not test from_map_server.launch as we don't want to add dependency on map_server and this
-    COMMAND bash download.sh ${CMAKE_BINARY_DIR}/map00.pgm ${base_url}/hector_maps/map00.pgm 915609a85793ec1375f310d44f2daf87
+#   # launchfile is not critical
-    WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test
+#   roslaunch_add_file_check(launch/map_merge.launch)
-  )
+#   roslaunch_add_file_check(launch/experiments)
-  execute_process(
+# endif()
-    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()

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_updates
+    robot_map_updates_topic: map_udpates
     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

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(

map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py

@@ -23,6 +23,7 @@ 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,
@@ -34,7 +35,6 @@ 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 beginning
+    # 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},
@@ -159,105 +159,39 @@ 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):
-        # after humble release, use spawn_entity.py
+        spawn_robots_cmds.append(
-        if os.getenv("ROS_DISTRO") == "humble":
+            IncludeLaunchDescription(
-            spawn_robots_cmds.append(
+                PythonLaunchDescriptionSource(
-                Node(
+                    os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
-                    package="gazebo_ros",
+                ),
-                    executable="spawn_entity.py",
+                launch_arguments={
-                    output="screen",
+                    "x_pose": TextSubstitution(text=str(robot_known["x_pose"])),
-                    arguments=[
+                    "y_pose": TextSubstitution(text=str(robot_known["y_pose"])),
-                        "-entity",
+                    "z_pose": TextSubstitution(text=str(robot_known["z_pose"])),
-                        robot_known["name"],
+                    "robot_name": robot_known["name"],
-                        "-file",
+                    "turtlebot_type": TextSubstitution(text="waffle"),
-                        robot_sdf,
+                }.items(),
-                        "-robot_namespace",
+                condition=IfCondition(known_init_poses),
-                        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(
+        )
-                Node(
+        spawn_robots_cmds.append(
-                    package="gazebo_ros",
+            IncludeLaunchDescription(
-                    executable="spawn_entity.py",
+                PythonLaunchDescriptionSource(
-                    output="screen",
+                    os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
-                    arguments=[
+                ),
-                        "-entity",
+                launch_arguments={
-                        robot_unknown["name"],
+                    "x_pose": TextSubstitution(text=str(robot_unknown["x_pose"])),
-                        "-file",
+                    "y_pose": TextSubstitution(text=str(robot_unknown["y_pose"])),
-                        robot_sdf,
+                    "z_pose": TextSubstitution(text=str(robot_unknown["z_pose"])),
-                        "-robot_namespace",
+                    "robot_name": robot_unknown["name"],
-                        TextSubstitution(text=str(robot_unknown["name"])),
+                    "turtlebot_type": TextSubstitution(text="waffle"),
-                        "-x",
+                }.items(),
-                        TextSubstitution(text=str(robot_unknown["x_pose"])),
+                condition=UnlessCondition(known_init_poses),
-                        "-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 = []
@@ -346,7 +280,6 @@ 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)

map_merge/package.xml

@@ -20,8 +20,6 @@
   <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>
 

map_merge/src/combine_grids/merging_pipeline.cpp

@@ -101,27 +101,28 @@ 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) {
-    transforms_.clear();
+    if (images_.size() != transforms_.size()) {
-    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;
-      }
     }
-    // RCLCPP_INFO(logger, "No match found between maps, setting first non-empty grid as reference frame");
+    return false;
-    return true;
   }
 
   // // Experimental: should we keep only the best confidence match overall?

map_merge/test/download.sh

@@ -1,21 +0,0 @@
-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

map_merge/test/download_data.sh

@@ -1,5 +0,0 @@
-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

map_merge/test/test_merging_pipeline.cpp

@@ -3,7 +3,6 @@
  * 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
@@ -37,7 +36,8 @@
 
 #include <combine_grids/grid_warper.h>
 #include <gtest/gtest.h>
-// #include <ros/console.h>
+#include <ros/console.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.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::msg::OccupancyGrid::ConstSharedPtr> maps;
+  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(), (long unsigned int) 0);
+  EXPECT_EQ(merger.getTransforms().size(), 0);
 }
 
 TEST(MergingPipeline, canStich1Grid)
@@ -82,11 +82,10 @@ TEST(MergingPipeline, canStich1Grid)
 
   EXPECT_VALID_GRID(merged_grid);
   // don't use EXPECT_EQ, since it prints too much info
-  EXPECT_TRUE(maps_equal(*merged_grid, *map));
+  EXPECT_TRUE(*merged_grid == *map);
-
   // check estimated transforms
   auto transforms = merger.getTransforms();
-  EXPECT_EQ(transforms.size(), (long unsigned int) 1);
+  EXPECT_EQ(transforms.size(), 1);
   EXPECT_TRUE(isIdentity(transforms[0]));
 }
 
@@ -157,7 +156,7 @@ TEST(MergingPipeline, estimationAccuracy)
   EXPECT_VALID_GRID(merged_grid);
   // transforms
   auto transforms = merger.getTransforms();
-  EXPECT_EQ(transforms.size(), (long unsigned int) 2);
+  EXPECT_EQ(transforms.size(), 2);
   EXPECT_TRUE(isIdentity(transforms[0]));
   tf2::Transform t;
   tf2::fromMsg(transforms[1], t);
@@ -177,7 +176,7 @@ TEST(MergingPipeline, transformsRoundTrip)
     merger.setTransforms(&in_transform, &in_transform + 1);
 
     auto out_transforms = merger.getTransforms();
-    ASSERT_EQ(out_transforms.size(), (long unsigned int) 1);
+    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);
@@ -199,7 +198,7 @@ TEST(MergingPipeline, setTransformsInternal)
     auto transform = randomTransform();
     merger.setTransforms(&transform, &transform + 1);
 
-    ASSERT_EQ(merger.transforms_.size(), (long unsigned int) 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.}};
@@ -229,7 +228,7 @@ TEST(MergingPipeline, getTransformsInternal)
     cv::Mat transform_internal = randomTransformMatrix();
     merger.transforms_[0] = transform_internal;
     auto transforms = merger.getTransforms();
-    ASSERT_EQ(transforms.size(), (long unsigned int) 1);
+    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);
@@ -251,8 +250,8 @@ TEST(MergingPipeline, getTransformsInternal)
 TEST(MergingPipeline, setEmptyTransforms)
 {
   constexpr size_t size = 2;
-  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps(size);
+  std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
-  std::vector<geometry_msgs::msg::Transform> transforms(size);
+  std::vector<geometry_msgs::Transform> transforms(size);
   combine_grids::MergingPipeline merger;
   merger.feed(maps.begin(), maps.end());
   merger.setTransforms(transforms.begin(), transforms.end());
@@ -264,8 +263,8 @@ TEST(MergingPipeline, setEmptyTransforms)
 TEST(MergingPipeline, emptyImageWithTransform)
 {
   constexpr size_t size = 1;
-  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps(size);
+  std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
-  std::vector<geometry_msgs::msg::Transform> transforms(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());
@@ -277,7 +276,7 @@ TEST(MergingPipeline, emptyImageWithTransform)
 /* one image may be empty */
 TEST(MergingPipeline, oneEmptyImage)
 {
-  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps{nullptr,
+  std::vector<nav_msgs::OccupancyGridConstPtr> maps{nullptr,
                                                     loadMap(gmapping_maps[0])};
   combine_grids::MergingPipeline merger;
   merger.feed(maps.begin(), maps.end());
@@ -287,9 +286,9 @@ TEST(MergingPipeline, oneEmptyImage)
 
   EXPECT_VALID_GRID(merged_grid);
   // don't use EXPECT_EQ, since it prints too much info
-  EXPECT_TRUE(maps_equal(*merged_grid, *maps[1]));
+  EXPECT_TRUE(*merged_grid == *maps[1]);
   // transforms
-  EXPECT_EQ(transforms.size(), (long unsigned int) 2);
+  EXPECT_EQ(transforms.size(), 2);
   EXPECT_TRUE(isIdentity(transforms[1]));
 }
 
@@ -301,7 +300,7 @@ TEST(MergingPipeline, knownInitPositions)
   merger.feed(maps.begin(), maps.end());
 
   for (size_t i = 0; i < 5; ++i) {
-    std::vector<geometry_msgs::msg::Transform> transforms{randomTransform(),
+    std::vector<geometry_msgs::Transform> transforms{randomTransform(),
                                                      randomTransform()};
     merger.setTransforms(transforms.begin(), transforms.end());
     auto merged_grid = merger.composeGrids();
@@ -312,12 +311,12 @@ TEST(MergingPipeline, knownInitPositions)
 
 int main(int argc, char** argv)
 {
-  // ros::Time::init();
+  ros::Time::init();
-  // if (verbose_tests &&
+  if (verbose_tests &&
-  //     ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
+      ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
-  //                                    ros::console::levels::Debug)) {
+                                     ros::console::levels::Debug)) {
-  //   ros::console::notifyLoggerLevelsChanged();
+    ros::console::notifyLoggerLevelsChanged();
-  // }
+  }
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }

map_merge/test/testing_helpers.h

@@ -1,33 +1,28 @@
 #ifndef TESTING_HELPERS_H_
 #define TESTING_HELPERS_H_
 
-#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/OccupancyGrid.h>
-#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::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename);
+nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename);
 void saveMap(const std::string& filename,
-             const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map);
+             const nav_msgs::OccupancyGridConstPtr& map);
 std::tuple<double, double, double> randomAngleTxTy();
-geometry_msgs::msg::Transform randomTransform();
+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::msg::OccupancyGrid::ConstSharedPtr> loadMaps(InputIt filenames_begin,
+std::vector<nav_msgs::OccupancyGridConstPtr> loadMaps(InputIt filenames_begin,
                                                       InputIt filenames_end)
 {
-  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> result;
+  std::vector<nav_msgs::OccupancyGridConstPtr> result;
 
   for (InputIt it = filenames_begin; it != filenames_end; ++it) {
     result.emplace_back(loadMap(*it));
@@ -35,7 +30,7 @@ std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> loadMaps(InputIt filen
   return result;
 }
 
-nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename)
+nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename)
 {
   cv::Mat lookUpTable(1, 256, CV_8S);
   signed char* p = lookUpTable.ptr<signed char>();
@@ -47,7 +42,7 @@ nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename
   if (img.empty()) {
     throw std::runtime_error("could not load map");
   }
-  nav_msgs::msg::OccupancyGrid::SharedPtr grid{new nav_msgs::msg::OccupancyGrid()};
+  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;
@@ -60,7 +55,7 @@ nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename
 }
 
 void saveMap(const std::string& filename,
-             const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map)
+             const nav_msgs::OccupancyGridConstPtr& map)
 {
   cv::Mat lookUpTable(1, 256, CV_8U);
   uchar* p = lookUpTable.ptr();
@@ -89,7 +84,7 @@ std::tuple<double, double, double> randomAngleTxTy()
                                             translation_dis(g));
 }
 
-geometry_msgs::msg::Transform randomTransform()
+geometry_msgs::Transform randomTransform()
 {
   double angle, tx, ty;
   std::tie(angle, tx, ty) = randomAngleTxTy();
@@ -124,14 +119,14 @@ cv::Mat randomTransformMatrix()
   return transform;
 }
 
-static inline bool isIdentity(const geometry_msgs::msg::Transform& 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::msg::Quaternion& rotation)
+static inline bool isIdentity(const geometry_msgs::Quaternion& rotation)
 {
   tf2::Quaternion q;
   tf2::fromMsg(rotation, q);
@@ -139,32 +134,15 @@ static inline bool isIdentity(const geometry_msgs::msg::Quaternion& rotation)
 }
 
 // data size is consistent with height and width
-static inline bool consistentData(const nav_msgs::msg::OccupancyGrid& grid)
+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::msg::OccupancyGrid::SharedPtr grid1,
+static inline bool operator==(const nav_msgs::OccupancyGrid& grid1,
-//                               const nav_msgs::msg::OccupancyGrid::SharedPtr grid2)
+                              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;
-// }
-
-// 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;