test_map.cpp
Go to the documentation of this file.
1 /*
2  * test_map.cpp
3  *
4  * Created on: July 21, 2014
5  * Author: Markus Bader
6  */
7 
8 #include <gtest/gtest.h>
9 #include <mrpt_bridge/map.h>
10 #define __STDC_FORMAT_MACROS
11 #include <inttypes.h>
12 
13 #include <nav_msgs/OccupancyGrid.h>
14 #include <ros/console.h>
15 
16 #include <mrpt/version.h>
17 #include <mrpt/maps/COccupancyGridMap2D.h>
19 
21 {
22  msg.info.width = 300;
23  msg.info.height = 500;
24  msg.info.resolution = 0.1;
25  msg.info.origin.position.x = rand() % 10 - 5;
26  msg.info.origin.position.y = rand() % 10 - 5;
27  msg.info.origin.position.z = 0;
28 
29  msg.info.origin.orientation.x = 0;
30  msg.info.origin.orientation.y = 0;
31  msg.info.origin.orientation.z = 0;
32  msg.info.origin.orientation.w = 1;
33 
34  msg.data.resize(msg.info.width * msg.info.height, -1);
35 }
36 
37 TEST(Map, basicTestHeader)
38 {
40  COccupancyGridMap2D desMrpt;
41 
42  getEmptyRosMsg(srcRos);
43 
44  srcRos.info.origin.orientation.x = 1; // roated maps are not supported
45  EXPECT_FALSE(mrpt_bridge::convert(srcRos, desMrpt));
46  srcRos.info.origin.orientation.x = 0; // fix rotation
47  EXPECT_TRUE(mrpt_bridge::convert(srcRos, desMrpt));
48 
49  EXPECT_EQ(srcRos.info.width, desMrpt.getSizeX());
50  EXPECT_EQ(srcRos.info.height, desMrpt.getSizeY());
51  EXPECT_EQ(srcRos.info.resolution, desMrpt.getResolution());
52  for (uint32_t h = 0; h < srcRos.info.width; h++)
53  {
54  for (uint32_t w = 0; w < srcRos.info.width; w++)
55  {
56  EXPECT_EQ(
57  desMrpt.getPos(w, h), 0.5); // all -1 entreis should map to 0.5
58  }
59  }
60 }
61 
62 TEST(Map, check_ros2mrpt_and_back)
63 {
65  COccupancyGridMap2D desMrpt;
67 
68  getEmptyRosMsg(srcRos);
69 
70  ASSERT_TRUE(mrpt_bridge::convert(srcRos, desMrpt));
71  ASSERT_TRUE(mrpt_bridge::convert(desMrpt, desRos, desRos.header));
72  for (uint32_t h = 0; h < srcRos.info.width; h++)
73  {
74  for (uint32_t w = 0; w < srcRos.info.width; w++)
75  {
76  EXPECT_EQ(
77  desRos.data[h * srcRos.info.width + h],
78  50); // all -1 entreis should map to 50
79  }
80  }
81 
82  for (int i = 0; i <= 100; i++)
83  {
84  srcRos.data[i] = i;
85  }
86  EXPECT_TRUE(mrpt_bridge::convert(srcRos, desMrpt));
87  EXPECT_TRUE(mrpt_bridge::convert(desMrpt, desRos, desRos.header));
88  for (int i = 0; i <= 100; i++)
89  {
90  // printf("%4i, %4.3f = %4.3f,%4i\n", srcRos.data[i],
91  // 1-((float)i)/100.0, desMrpt.getCell(i,0), desRos.data[i]);
92  EXPECT_NEAR(1 - ((float)i) / 100.0, desMrpt.getCell(i, 0), 0.03)
93  << "ros to mprt";
94  EXPECT_NEAR(srcRos.data[i], desRos.data[i], 1) << "ros to mprt to ros";
95  }
96 }
GLubyte GLubyte GLubyte GLubyte w
unsigned int uint32_t
#define EXPECT_NEAR(val1, val2, abs_error)
#define ASSERT_TRUE(condition)
TEST(Map, basicTestHeader)
Definition: test_map.cpp:37
bool convert(const mrpt_msgs::ObservationRangeBeacon &_msg, const mrpt::poses::CPose3D &_pose, mrpt::obs::CObservationBeaconRanges &_obj)
Definition: beacon.cpp:24
unsigned int getSizeY() const
unsigned int getSizeX() const
float getCell(int x, int y) const
#define EXPECT_EQ(val1, val2)
void getEmptyRosMsg(nav_msgs::OccupancyGrid &msg)
Definition: test_map.cpp:20
#define EXPECT_TRUE(condition)
#define EXPECT_FALSE(condition)
float getPos(float x, float y) const


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Fri Feb 28 2020 03:22:14