MRPT  1.9.9
map_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /*
11  * test_map.cpp
12  *
13  * Created on: July 21, 2014
14  * Author: Markus Bader
15  */
16 
17 #include <gtest/gtest.h>
19 #include <mrpt/ros1bridge/map.h>
20 #include <nav_msgs/OccupancyGrid.h>
21 #include <ros/console.h>
22 
24 
25 void getEmptyRosMsg(nav_msgs::OccupancyGrid& msg)
26 {
27  msg.info.width = 300;
28  msg.info.height = 500;
29  msg.info.resolution = 0.1;
30  msg.info.origin.position.x = rand() % 10 - 5;
31  msg.info.origin.position.y = rand() % 10 - 5;
32  msg.info.origin.position.z = 0;
33 
34  msg.info.origin.orientation.x = 0;
35  msg.info.origin.orientation.y = 0;
36  msg.info.origin.orientation.z = 0;
37  msg.info.origin.orientation.w = 1;
38 
39  msg.data.resize(msg.info.width * msg.info.height, -1);
40 }
41 
42 TEST(Map, basicTestHeader)
43 {
44  nav_msgs::OccupancyGrid srcRos;
45  COccupancyGridMap2D desMrpt;
46 
47  getEmptyRosMsg(srcRos);
48 
49  srcRos.info.origin.orientation.x = 0; // fix rotation
50  EXPECT_TRUE(mrpt::ros1bridge::fromROS(srcRos, desMrpt));
51 
52  EXPECT_EQ(srcRos.info.width, desMrpt.getSizeX());
53  EXPECT_EQ(srcRos.info.height, desMrpt.getSizeY());
54  EXPECT_EQ(srcRos.info.resolution, desMrpt.getResolution());
55  for (uint32_t h = 0; h < srcRos.info.width; h++)
56  {
57  for (uint32_t w = 0; w < srcRos.info.width; w++)
58  {
59  EXPECT_EQ(
60  desMrpt.getPos(w, h), 0.5); // all -1 entreis should map to 0.5
61  }
62  }
63 }
64 
65 TEST(Map, check_ros2mrpt_and_back)
66 {
67  nav_msgs::OccupancyGrid srcRos;
68  COccupancyGridMap2D desMrpt;
69  nav_msgs::OccupancyGrid desRos;
70 
71  // Test empty gridmap:
72  getEmptyRosMsg(srcRos);
73 
74  ASSERT_TRUE(mrpt::ros1bridge::fromROS(srcRos, desMrpt));
75  ASSERT_TRUE(mrpt::ros1bridge::toROS(desMrpt, desRos, desRos.header));
76  // all -1 entries should map back to -1
77  for (uint32_t h = 0; h < srcRos.info.width; h++)
78  for (uint32_t w = 0; w < srcRos.info.width; w++)
79  EXPECT_EQ(desRos.data[h * srcRos.info.width + h], -1);
80 
81  // Test gridmap with values: 0 to 100
82  for (int i = 0; i <= 100; i++)
83  {
84  // 50 is mid-gray -> unknown = -1 in ROS
85  srcRos.data[i] = (std::abs(i - 50) <= 2) ? -1 : i;
86  }
87 
88  EXPECT_TRUE(mrpt::ros1bridge::fromROS(srcRos, desMrpt));
89  EXPECT_TRUE(mrpt::ros1bridge::toROS(desMrpt, desRos, desRos.header));
90 
91  for (int i = 0; i <= 100; i++)
92  {
93  /*printf(
94  "%4i, %4.3f = %4.3f,%4i\n", srcRos.data[i], 1.0f - 0.01f * i,
95  desMrpt.getCell(i, 0), desRos.data[i]); */
96  EXPECT_NEAR(1.0f - 0.01f * i, desMrpt.getCell(i, 0), 0.03f)
97  << "ros to mprt"
98  << "i=" << i;
99  EXPECT_NEAR(srcRos.data[i], desRos.data[i], 1) << "ros to mprt to ros";
100  }
101 }
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
float getResolution() const
Returns the resolution of the grid map.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
TEST(Map, basicTestHeader)
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
A class for storing an occupancy grid map.
bool toROS(const mrpt::obs::CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Convert mrpt::obs::CObservationGPS -> sensor_msgs/NavSatFix The user must supply the "msg_header" fie...
Definition: gps.cpp:48
bool fromROS(const sensor_msgs::NavSatFix &msg, mrpt::obs::CObservationGPS &obj)
Convert sensor_msgs/NavSatFix -> mrpt::obs::CObservationGPS.
Definition: gps.cpp:20
void getEmptyRosMsg(nav_msgs::OccupancyGrid &msg)
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 45d659fbb Tue Dec 10 18:21:14 2019 +0100 at mar dic 10 18:30:09 CET 2019