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

.github/workflows/build_test.yaml

@@ -0,0 +1,22 @@
+name: ROS2 build/tests 
+on:
+  pull_request:
+jobs:
+  build_and_tests:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: ros-tooling/setup-ros@v0.3
+        with:
+          required-ros-distributions: galactic
+      - name: Checkout repository
+        uses: actions/checkout@v3
+      - uses: ros-tooling/action-ros-ci@v0.2
+        with:
+          package-name: multirobot_map_merge
+          target-ros2-distro: galactic
+          skip-tests: true
+      - name: Run gtests manually
+        run: |
+          source /opt/ros/galactic/setup.sh && source ros_ws/install/setup.sh
+          cd ros_ws/build/multirobot_map_merge
+          ./test_merging_pipeline

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
 )
 
@@ -110,35 +114,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)
+if(BUILD_TESTING)
-#   find_package(roslaunch REQUIRED)
+  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
+  find_package(ament_cmake_gtest REQUIRED)
-#   set(base_url https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge)
-#   catkin_download_test_data(${PROJECT_NAME}_map00.pgm ${base_url}/hector_maps/map00.pgm MD5 915609a85793ec1375f310d44f2daf87)
-#   catkin_download_test_data(${PROJECT_NAME}_map05.pgm ${base_url}/hector_maps/map05.pgm MD5 cb9154c9fa3d97e5e992592daca9853a)
-#   catkin_download_test_data(${PROJECT_NAME}_2011-08-09-12-22-52.pgm ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm MD5 3c2c38e7dec2b7a67f41069ab58badaa)
-#   catkin_download_test_data(${PROJECT_NAME}_2012-01-28-11-12-01.pgm ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm MD5 681e704044889c95e47b0c3aadd81f1e)
 
-#   catkin_add_gtest(test_merging_pipeline test/test_merging_pipeline.cpp)
+  # download test data: TODO: ROS2 alternative? For now you'll need to download them manually
-#   # ensure that test data are downloaded before we run tests
+  set(base_url https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge)
-#   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)
+  # ament_download(${base_url}/hector_maps/map00.pgm
-#   target_link_libraries(test_merging_pipeline combine_grids ${catkin_LIBRARIES})
+  #   MD5 915609a85793ec1375f310d44f2daf87
-
+  #   FILENAME ${PROJECT_NAME}_map00.pgm
-#   # test all launch files
+  # )
-#   # do not test from_map_server.launch as we don't want to add dependency on map_server and this
+  execute_process(
-#   # launchfile is not critical
+    COMMAND bash download.sh ${CMAKE_BINARY_DIR}/map00.pgm ${base_url}/hector_maps/map00.pgm 915609a85793ec1375f310d44f2daf87
-#   roslaunch_add_file_check(launch/map_merge.launch)
+    WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test
-#   roslaunch_add_file_check(launch/experiments)
+  )
-# endif()
+  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()

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>
 

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_.clear();
+    transforms_.resize(images_.size());
-      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?

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

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

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 <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::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<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps(size);
-  std::vector<geometry_msgs::Transform> transforms(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<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps(size);
-  std::vector<geometry_msgs::Transform> transforms(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();
+  // 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,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,
+// static inline bool operator==(const nav_msgs::msg::OccupancyGrid::SharedPtr grid1,
-                              const nav_msgs::OccupancyGrid& grid2)
+//                               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;