Go to the documentation of this file.00001
00002
00003
00004
00005
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;
00044 EXPECT_FALSE( mrpt_bridge::convert(srcRos, desMrpt));
00045 srcRos.info.origin.orientation.x = 0;
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);
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);
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
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 }