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;