test_map.cpp
Go to the documentation of this file.
00001 /*
00002  * test_map.cpp
00003  *
00004  *  Created on: July 21, 2014
00005  *      Author: Markus Bader
00006  */
00007 
00008 #include <gtest/gtest.h>
00009 #include <mrpt_bridge/map.h>
00010 #define __STDC_FORMAT_MACROS
00011 #include <inttypes.h>
00012 
00013 #include <nav_msgs/OccupancyGrid.h>
00014 #include <ros/console.h>
00015 
00016 #include <mrpt/version.h>
00017 #include <mrpt/maps/COccupancyGridMap2D.h>
00018 using mrpt::maps::COccupancyGridMap2D;
00019 
00020 void getEmptyRosMsg(nav_msgs::OccupancyGrid& msg)
00021 {
00022         msg.info.width = 300;
00023         msg.info.height = 500;
00024         msg.info.resolution = 0.1;
00025         msg.info.origin.position.x = rand() % 10 - 5;
00026         msg.info.origin.position.y = rand() % 10 - 5;
00027         msg.info.origin.position.z = 0;
00028 
00029         msg.info.origin.orientation.x = 0;
00030         msg.info.origin.orientation.y = 0;
00031         msg.info.origin.orientation.z = 0;
00032         msg.info.origin.orientation.w = 1;
00033 
00034         msg.data.resize(msg.info.width * msg.info.height, -1);
00035 }
00036 
00037 TEST(Map, basicTestHeader)
00038 {
00039         nav_msgs::OccupancyGrid srcRos;
00040         COccupancyGridMap2D desMrpt;
00041 
00042         getEmptyRosMsg(srcRos);
00043 
00044         srcRos.info.origin.orientation.x = 1;  // roated maps are not supported
00045         EXPECT_FALSE(mrpt_bridge::convert(srcRos, desMrpt));
00046         srcRos.info.origin.orientation.x = 0;  // fix rotation
00047         EXPECT_TRUE(mrpt_bridge::convert(srcRos, desMrpt));
00048 
00049         EXPECT_EQ(srcRos.info.width, desMrpt.getSizeX());
00050         EXPECT_EQ(srcRos.info.height, desMrpt.getSizeY());
00051         EXPECT_EQ(srcRos.info.resolution, desMrpt.getResolution());
00052         for (int h = 0; h < srcRos.info.width; h++)
00053         {
00054                 for (int w = 0; w < srcRos.info.width; w++)
00055                 {
00056                         EXPECT_EQ(
00057                                 desMrpt.getPos(w, h), 0.5);  // all -1 entreis should map to 0.5
00058                 }
00059         }
00060 }
00061 
00062 TEST(Map, check_ros2mrpt_and_back)
00063 {
00064         nav_msgs::OccupancyGrid srcRos;
00065         COccupancyGridMap2D desMrpt;
00066         nav_msgs::OccupancyGrid desRos;
00067 
00068         getEmptyRosMsg(srcRos);
00069 
00070         ASSERT_TRUE(mrpt_bridge::convert(srcRos, desMrpt));
00071         ASSERT_TRUE(mrpt_bridge::convert(desMrpt, desRos, desRos.header));
00072         for (int h = 0; h < srcRos.info.width; h++)
00073         {
00074                 for (int w = 0; w < srcRos.info.width; w++)
00075                 {
00076                         EXPECT_EQ(
00077                                 desRos.data[h * srcRos.info.width + h],
00078                                 50);  // all -1 entreis should map to 50
00079                 }
00080         }
00081 
00082         for (int i = 0; i <= 100; i++)
00083         {
00084                 srcRos.data[i] = i;
00085         }
00086         EXPECT_TRUE(mrpt_bridge::convert(srcRos, desMrpt));
00087         EXPECT_TRUE(mrpt_bridge::convert(desMrpt, desRos, desRos.header));
00088         for (int i = 0; i <= 100; i++)
00089         {
00090                 // printf("%4i, %4.3f = %4.3f,%4i\n", srcRos.data[i],
00091                 // 1-((float)i)/100.0, desMrpt.getCell(i,0), desRos.data[i]);
00092                 EXPECT_NEAR(1 - ((float)i) / 100.0, desMrpt.getCell(i, 0), 0.03)
00093                         << "ros to mprt";
00094                 EXPECT_NEAR(srcRos.data[i], desRos.data[i], 1) << "ros to mprt to ros";
00095         }
00096 }


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Fri Apr 27 2018 05:10:54