diff --git a/.github/workflows/build_test.yaml b/.github/workflows/build_test.yaml new file mode 100644 index 0000000..05487f4 --- /dev/null +++ b/.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 diff --git a/map_merge/CMakeLists.txt b/map_merge/CMakeLists.txt index 3786704..b9804be 100644 --- 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 ) @@ -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) -# 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() \ No newline at end of file diff --git a/map_merge/package.xml b/map_merge/package.xml index b97fe0a..a60e0a7 100644 --- a/map_merge/package.xml +++ b/map_merge/package.xml @@ -20,6 +20,8 @@ nav_msgs map_msgs tf2_geometry_msgs + tf2 + tf2_ros image_geometry diff --git a/map_merge/src/combine_grids/merging_pipeline.cpp b/map_merge/src/combine_grids/merging_pipeline.cpp index d4f66c1..bfbcef0 100644 --- 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? diff --git a/map_merge/test/download.sh b/map_merge/test/download.sh new file mode 100644 index 0000000..07496d5 --- /dev/null +++ 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 \ No newline at end of file diff --git a/map_merge/test/download_data.sh b/map_merge/test/download_data.sh new file mode 100644 index 0000000..574f53a --- /dev/null +++ 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 \ No newline at end of file diff --git a/map_merge/test/test_merging_pipeline.cpp b/map_merge/test/test_merging_pipeline.cpp index df856d0..d3066e5 100644 --- 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 #include -#include -#include +// #include #include #include "testing_helpers.h" @@ -64,12 +64,12 @@ constexpr bool verbose_tests = false; TEST(MergingPipeline, canStich0Grid) { - std::vector maps; + std::vector 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 maps(size); - std::vector transforms(size); + std::vector maps(size); + std::vector 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 maps(size); - std::vector transforms(size); + std::vector maps(size); + std::vector 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 maps{nullptr, + std::vector 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 transforms{randomTransform(), + std::vector 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(); } diff --git a/map_merge/test/testing_helpers.h b/map_merge/test/testing_helpers.h index 6302b7b..bae04d3 100644 --- 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 +#include +#include +#include #include +#include +#include +#include #include #include #include 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 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 -std::vector loadMaps(InputIt filenames_begin, +std::vector loadMaps(InputIt filenames_begin, InputIt filenames_end) { - std::vector result; + std::vector result; for (InputIt it = filenames_begin; it != filenames_end; ++it) { result.emplace_back(loadMap(*it)); @@ -30,7 +35,7 @@ std::vector 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(); @@ -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(img.size().width); grid->info.height = static_cast(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 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::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;