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 testsmain
Carlos Andrés Álvarez Restrepo
GitHub
parent
5dfdb3a68e
commit
159004e0c9
|
|
@ -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
|
||||
|
|
@ -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()
|
||||
|
|
@ -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>
|
||||
|
||||
|
|
|
|||
|
|
@ -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?
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -3,6 +3,7 @@
|
|||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2015-2016, Jiri Horner.
|
||||
* Copyright (c) 2022, Carlos Alvarez.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
|
|
@ -36,8 +37,7 @@
|
|||
|
||||
#include <combine_grids/grid_warper.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <ros/console.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
// #include <ros/console.h>
|
||||
#include <opencv2/core/utility.hpp>
|
||||
#include "testing_helpers.h"
|
||||
|
||||
|
|
@ -64,12 +64,12 @@ constexpr bool verbose_tests = false;
|
|||
|
||||
TEST(MergingPipeline, canStich0Grid)
|
||||
{
|
||||
std::vector<nav_msgs::OccupancyGridConstPtr> maps;
|
||||
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps;
|
||||
combine_grids::MergingPipeline merger;
|
||||
merger.feed(maps.begin(), maps.end());
|
||||
EXPECT_TRUE(merger.estimateTransforms());
|
||||
EXPECT_EQ(merger.composeGrids(), nullptr);
|
||||
EXPECT_EQ(merger.getTransforms().size(), 0);
|
||||
EXPECT_EQ(merger.getTransforms().size(), (long unsigned int) 0);
|
||||
}
|
||||
|
||||
TEST(MergingPipeline, canStich1Grid)
|
||||
|
|
@ -82,10 +82,11 @@ TEST(MergingPipeline, canStich1Grid)
|
|||
|
||||
EXPECT_VALID_GRID(merged_grid);
|
||||
// don't use EXPECT_EQ, since it prints too much info
|
||||
EXPECT_TRUE(*merged_grid == *map);
|
||||
EXPECT_TRUE(maps_equal(*merged_grid, *map));
|
||||
|
||||
// check estimated transforms
|
||||
auto transforms = merger.getTransforms();
|
||||
EXPECT_EQ(transforms.size(), 1);
|
||||
EXPECT_EQ(transforms.size(), (long unsigned int) 1);
|
||||
EXPECT_TRUE(isIdentity(transforms[0]));
|
||||
}
|
||||
|
||||
|
|
@ -156,7 +157,7 @@ TEST(MergingPipeline, estimationAccuracy)
|
|||
EXPECT_VALID_GRID(merged_grid);
|
||||
// transforms
|
||||
auto transforms = merger.getTransforms();
|
||||
EXPECT_EQ(transforms.size(), 2);
|
||||
EXPECT_EQ(transforms.size(), (long unsigned int) 2);
|
||||
EXPECT_TRUE(isIdentity(transforms[0]));
|
||||
tf2::Transform t;
|
||||
tf2::fromMsg(transforms[1], t);
|
||||
|
|
@ -176,7 +177,7 @@ TEST(MergingPipeline, transformsRoundTrip)
|
|||
merger.setTransforms(&in_transform, &in_transform + 1);
|
||||
|
||||
auto out_transforms = merger.getTransforms();
|
||||
ASSERT_EQ(out_transforms.size(), 1);
|
||||
ASSERT_EQ(out_transforms.size(), (long unsigned int) 1);
|
||||
auto out_transform = out_transforms[0];
|
||||
EXPECT_FLOAT_EQ(in_transform.translation.x, out_transform.translation.x);
|
||||
EXPECT_FLOAT_EQ(in_transform.translation.y, out_transform.translation.y);
|
||||
|
|
@ -198,7 +199,7 @@ TEST(MergingPipeline, setTransformsInternal)
|
|||
auto transform = randomTransform();
|
||||
merger.setTransforms(&transform, &transform + 1);
|
||||
|
||||
ASSERT_EQ(merger.transforms_.size(), 1);
|
||||
ASSERT_EQ(merger.transforms_.size(), (long unsigned int) 1);
|
||||
auto& transform_internal = merger.transforms_[0];
|
||||
// verify that transforms are the same in 2D
|
||||
tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}};
|
||||
|
|
@ -228,7 +229,7 @@ TEST(MergingPipeline, getTransformsInternal)
|
|||
cv::Mat transform_internal = randomTransformMatrix();
|
||||
merger.transforms_[0] = transform_internal;
|
||||
auto transforms = merger.getTransforms();
|
||||
ASSERT_EQ(transforms.size(), 1);
|
||||
ASSERT_EQ(transforms.size(), (long unsigned int) 1);
|
||||
// output quaternion should be normalized
|
||||
auto& q = transforms[0].rotation;
|
||||
EXPECT_DOUBLE_EQ(1., q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
|
||||
|
|
@ -250,8 +251,8 @@ TEST(MergingPipeline, getTransformsInternal)
|
|||
TEST(MergingPipeline, setEmptyTransforms)
|
||||
{
|
||||
constexpr size_t size = 2;
|
||||
std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
|
||||
std::vector<geometry_msgs::Transform> transforms(size);
|
||||
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps(size);
|
||||
std::vector<geometry_msgs::msg::Transform> transforms(size);
|
||||
combine_grids::MergingPipeline merger;
|
||||
merger.feed(maps.begin(), maps.end());
|
||||
merger.setTransforms(transforms.begin(), transforms.end());
|
||||
|
|
@ -263,8 +264,8 @@ TEST(MergingPipeline, setEmptyTransforms)
|
|||
TEST(MergingPipeline, emptyImageWithTransform)
|
||||
{
|
||||
constexpr size_t size = 1;
|
||||
std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
|
||||
std::vector<geometry_msgs::Transform> transforms(size);
|
||||
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps(size);
|
||||
std::vector<geometry_msgs::msg::Transform> transforms(size);
|
||||
transforms[0].rotation.z = 1; // set transform to identity
|
||||
combine_grids::MergingPipeline merger;
|
||||
merger.feed(maps.begin(), maps.end());
|
||||
|
|
@ -276,7 +277,7 @@ TEST(MergingPipeline, emptyImageWithTransform)
|
|||
/* one image may be empty */
|
||||
TEST(MergingPipeline, oneEmptyImage)
|
||||
{
|
||||
std::vector<nav_msgs::OccupancyGridConstPtr> maps{nullptr,
|
||||
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps{nullptr,
|
||||
loadMap(gmapping_maps[0])};
|
||||
combine_grids::MergingPipeline merger;
|
||||
merger.feed(maps.begin(), maps.end());
|
||||
|
|
@ -286,9 +287,9 @@ TEST(MergingPipeline, oneEmptyImage)
|
|||
|
||||
EXPECT_VALID_GRID(merged_grid);
|
||||
// don't use EXPECT_EQ, since it prints too much info
|
||||
EXPECT_TRUE(*merged_grid == *maps[1]);
|
||||
EXPECT_TRUE(maps_equal(*merged_grid, *maps[1]));
|
||||
// transforms
|
||||
EXPECT_EQ(transforms.size(), 2);
|
||||
EXPECT_EQ(transforms.size(), (long unsigned int) 2);
|
||||
EXPECT_TRUE(isIdentity(transforms[1]));
|
||||
}
|
||||
|
||||
|
|
@ -300,7 +301,7 @@ TEST(MergingPipeline, knownInitPositions)
|
|||
merger.feed(maps.begin(), maps.end());
|
||||
|
||||
for (size_t i = 0; i < 5; ++i) {
|
||||
std::vector<geometry_msgs::Transform> transforms{randomTransform(),
|
||||
std::vector<geometry_msgs::msg::Transform> transforms{randomTransform(),
|
||||
randomTransform()};
|
||||
merger.setTransforms(transforms.begin(), transforms.end());
|
||||
auto merged_grid = merger.composeGrids();
|
||||
|
|
@ -311,12 +312,12 @@ TEST(MergingPipeline, knownInitPositions)
|
|||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::Time::init();
|
||||
if (verbose_tests &&
|
||||
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
|
||||
ros::console::levels::Debug)) {
|
||||
ros::console::notifyLoggerLevelsChanged();
|
||||
}
|
||||
// ros::Time::init();
|
||||
// if (verbose_tests &&
|
||||
// ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
|
||||
// ros::console::levels::Debug)) {
|
||||
// ros::console::notifyLoggerLevelsChanged();
|
||||
// }
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,28 +1,33 @@
|
|||
#ifndef TESTING_HELPERS_H_
|
||||
#define TESTING_HELPERS_H_
|
||||
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <nav_msgs/msg/occupancy_grid.hpp>
|
||||
#include <geometry_msgs/msg/transform.hpp>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf2/LinearMath/Transform.h>
|
||||
#include <tf2/convert.h>
|
||||
#include <opencv2/core/utility.hpp>
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
#include <random>
|
||||
|
||||
const float resolution = 0.05f;
|
||||
|
||||
nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename);
|
||||
nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename);
|
||||
void saveMap(const std::string& filename,
|
||||
const nav_msgs::OccupancyGridConstPtr& map);
|
||||
const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map);
|
||||
std::tuple<double, double, double> randomAngleTxTy();
|
||||
geometry_msgs::Transform randomTransform();
|
||||
geometry_msgs::msg::Transform randomTransform();
|
||||
cv::Mat randomTransformMatrix();
|
||||
|
||||
/* map_server is really bad. until there is no replacement I will implement it
|
||||
* by myself */
|
||||
template <typename InputIt>
|
||||
std::vector<nav_msgs::OccupancyGridConstPtr> loadMaps(InputIt filenames_begin,
|
||||
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> loadMaps(InputIt filenames_begin,
|
||||
InputIt filenames_end)
|
||||
{
|
||||
std::vector<nav_msgs::OccupancyGridConstPtr> result;
|
||||
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> result;
|
||||
|
||||
for (InputIt it = filenames_begin; it != filenames_end; ++it) {
|
||||
result.emplace_back(loadMap(*it));
|
||||
|
|
@ -30,7 +35,7 @@ std::vector<nav_msgs::OccupancyGridConstPtr> loadMaps(InputIt filenames_begin,
|
|||
return result;
|
||||
}
|
||||
|
||||
nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename)
|
||||
nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename)
|
||||
{
|
||||
cv::Mat lookUpTable(1, 256, CV_8S);
|
||||
signed char* p = lookUpTable.ptr<signed char>();
|
||||
|
|
@ -42,7 +47,7 @@ nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename)
|
|||
if (img.empty()) {
|
||||
throw std::runtime_error("could not load map");
|
||||
}
|
||||
nav_msgs::OccupancyGridPtr grid{new nav_msgs::OccupancyGrid()};
|
||||
nav_msgs::msg::OccupancyGrid::SharedPtr grid{new nav_msgs::msg::OccupancyGrid()};
|
||||
grid->info.width = static_cast<uint>(img.size().width);
|
||||
grid->info.height = static_cast<uint>(img.size().height);
|
||||
grid->info.resolution = resolution;
|
||||
|
|
@ -55,7 +60,7 @@ nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename)
|
|||
}
|
||||
|
||||
void saveMap(const std::string& filename,
|
||||
const nav_msgs::OccupancyGridConstPtr& map)
|
||||
const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map)
|
||||
{
|
||||
cv::Mat lookUpTable(1, 256, CV_8U);
|
||||
uchar* p = lookUpTable.ptr();
|
||||
|
|
@ -84,7 +89,7 @@ std::tuple<double, double, double> randomAngleTxTy()
|
|||
translation_dis(g));
|
||||
}
|
||||
|
||||
geometry_msgs::Transform randomTransform()
|
||||
geometry_msgs::msg::Transform randomTransform()
|
||||
{
|
||||
double angle, tx, ty;
|
||||
std::tie(angle, tx, ty) = randomAngleTxTy();
|
||||
|
|
@ -119,14 +124,14 @@ cv::Mat randomTransformMatrix()
|
|||
return transform;
|
||||
}
|
||||
|
||||
static inline bool isIdentity(const geometry_msgs::Transform& transform)
|
||||
static inline bool isIdentity(const geometry_msgs::msg::Transform& transform)
|
||||
{
|
||||
tf2::Transform t;
|
||||
tf2::fromMsg(transform, t);
|
||||
return tf2::Transform::getIdentity() == t;
|
||||
}
|
||||
|
||||
static inline bool isIdentity(const geometry_msgs::Quaternion& rotation)
|
||||
static inline bool isIdentity(const geometry_msgs::msg::Quaternion& rotation)
|
||||
{
|
||||
tf2::Quaternion q;
|
||||
tf2::fromMsg(rotation, q);
|
||||
|
|
@ -134,15 +139,32 @@ static inline bool isIdentity(const geometry_msgs::Quaternion& rotation)
|
|||
}
|
||||
|
||||
// data size is consistent with height and width
|
||||
static inline bool consistentData(const nav_msgs::OccupancyGrid& grid)
|
||||
static inline bool consistentData(const nav_msgs::msg::OccupancyGrid& grid)
|
||||
{
|
||||
return grid.info.width * grid.info.height == grid.data.size();
|
||||
}
|
||||
|
||||
// ignores header, map_load_time and origin
|
||||
static inline bool operator==(const nav_msgs::OccupancyGrid& grid1,
|
||||
const nav_msgs::OccupancyGrid& grid2)
|
||||
// static inline bool operator==(const nav_msgs::msg::OccupancyGrid::SharedPtr grid1,
|
||||
// const nav_msgs::msg::OccupancyGrid::SharedPtr grid2)
|
||||
// {
|
||||
// bool equal = true;
|
||||
// equal &= grid1->info.width == grid2->info.width;
|
||||
// equal &= grid1->info.height == grid2->info.height;
|
||||
// equal &= std::abs(grid1->info.resolution - grid2->info.resolution) <
|
||||
// std::numeric_limits<float>::epsilon();
|
||||
// equal &= grid1->data.size() == grid2->data.size();
|
||||
// for (size_t i = 0; i < grid1->data.size(); ++i) {
|
||||
// equal &= grid1->data[i] == grid2->data[i];
|
||||
// }
|
||||
// return equal;
|
||||
// }
|
||||
|
||||
// ignores header, map_load_time and origin
|
||||
static inline bool maps_equal(const nav_msgs::msg::OccupancyGrid& grid1,
|
||||
const nav_msgs::msg::OccupancyGrid& grid2)
|
||||
{
|
||||
// std::cout << "asdasdadsdth: " << std::endl;
|
||||
bool equal = true;
|
||||
equal &= grid1.info.width == grid2.info.width;
|
||||
equal &= grid1.info.height == grid2.info.height;
|
||||
|
|
|
|||
Loading…
Reference in New Issue