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 <mrpt/slam/COccupancyGridMap2D.h>
00015 #include <ros/console.h>
00016 
00017 void getEmptyRosMsg(nav_msgs::OccupancyGrid &msg){    
00018   
00019     msg.info.width = 300;
00020     msg.info.height = 500;
00021     msg.info.resolution = 0.1;
00022     msg.info.origin.position.x = rand() % 10 - 5;
00023     msg.info.origin.position.y = rand() % 10 - 5;
00024     msg.info.origin.position.z = 0;
00025 
00026     msg.info.origin.orientation.x = 0;
00027     msg.info.origin.orientation.y = 0;
00028     msg.info.origin.orientation.z = 0;
00029     msg.info.origin.orientation.w = 1;
00030 
00031     msg.data.resize(msg.info.width * msg.info.height, -1);
00032 }
00033 
00034 TEST(Map, basicTestHeader)
00035 {
00036 
00037     nav_msgs::OccupancyGrid  srcRos;
00038     mrpt::slam::COccupancyGridMap2D  desMrpt;
00039 
00040     getEmptyRosMsg(srcRos);
00041 
00042 
00043     srcRos.info.origin.orientation.x = 1; // roated maps are not supported
00044     EXPECT_FALSE( mrpt_bridge::convert(srcRos, desMrpt));
00045     srcRos.info.origin.orientation.x = 0; // fix rotation
00046     EXPECT_TRUE( mrpt_bridge::convert(srcRos, desMrpt));
00047 
00048     EXPECT_EQ(srcRos.info.width, desMrpt.getSizeX());
00049     EXPECT_EQ(srcRos.info.height, desMrpt.getSizeY());
00050     EXPECT_EQ(srcRos.info.resolution, desMrpt.getResolution());
00051     for(int h = 0; h < srcRos.info.width; h++) {
00052         for(int w = 0; w < srcRos.info.width; w++) {
00053             EXPECT_EQ(desMrpt.getPos(w,h), 0.5); // all -1 entreis should map to 0.5
00054         }
00055     }
00056 }
00057 
00058 TEST(Map, check_ros2mrpt_and_back)
00059 {
00060 
00061     nav_msgs::OccupancyGrid  srcRos;
00062     mrpt::slam::COccupancyGridMap2D  desMrpt;
00063     nav_msgs::OccupancyGrid  desRos;
00064 
00065     getEmptyRosMsg(srcRos);
00066 
00067 
00068     ASSERT_TRUE( mrpt_bridge::convert(srcRos, desMrpt));
00069     ASSERT_TRUE( mrpt_bridge::convert(desMrpt, desRos, desRos.header));
00070     for(int h = 0; h < srcRos.info.width; h++) {
00071         for(int w = 0; w < srcRos.info.width; w++) {
00072           EXPECT_EQ(desRos.data[h*srcRos.info.width+h], 50); // all -1 entreis should map to 50
00073         }
00074     }  
00075     
00076     for(int i = 0; i <= 100; i++){
00077       srcRos.data[i] = i;
00078     }
00079     EXPECT_TRUE( mrpt_bridge::convert(srcRos, desMrpt));
00080     EXPECT_TRUE( mrpt_bridge::convert(desMrpt, desRos, desRos.header));
00081     for(int i = 0; i <= 100; i++){
00082           //printf("%4i, %4.3f = %4.3f,%4i\n", srcRos.data[i], 1-((float)i)/100.0, desMrpt.getCell(i,0), desRos.data[i]);
00083           EXPECT_NEAR(1-((float)i)/100.0, desMrpt.getCell(i,0), 0.03) << "ros to mprt";  
00084           EXPECT_NEAR(srcRos.data[i], desRos.data[i], 1) << "ros to mprt to ros"; 
00085     }
00086 }


mrpt_bridge
Author(s):
autogenerated on Mon Aug 11 2014 11:23:21