GridMapRosTest.cpp
Go to the documentation of this file.
1 /*
2  * GridMapRosTest.cpp
3  *
4  * Created on: Mar 24, 2015
5  * Author: Peter Fankhauser, Martin Wermelinger
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
13 #include "grid_map_msgs/GridMap.h"
14 
15 // gtest
16 #include <gtest/gtest.h>
17 
18 // Eigen
19 #include <Eigen/Core>
20 
21 // STD
22 #include <string>
23 #include <vector>
24 #include <stdlib.h>
25 #include <iterator>
26 
27 // ROS
28 #include <nav_msgs/OccupancyGrid.h>
29 #include <cv_bridge/cv_bridge.h>
31 
32 using namespace std;
33 using namespace grid_map;
34 
35 TEST(RosMessageConversion, roundTrip)
36 {
37  GridMap mapIn({"layer"});
38  mapIn.setGeometry(Length(2.0, 3.0), 0.5, Position(1.0, 1.5));
39  mapIn["layer"].setRandom();
40 
41  grid_map_msgs::GridMap message;
42  GridMapRosConverter::toMessage(mapIn, message);
43  GridMap mapOut;
44  GridMapRosConverter::fromMessage(message, mapOut);
45 
46  for (size_t i = 0; i < mapIn.getLayers().size(); ++i) {
47  EXPECT_EQ(mapIn.getLayers().at(i), mapOut.getLayers().at(i));
48  const std::string layer = mapIn.getLayers().at(i);
49  EXPECT_TRUE((mapIn[layer].array() == mapOut[layer].array()).all());
50  }
51 
52  EXPECT_EQ(mapIn.getFrameId(), mapOut.getFrameId());
53  EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all());
54  EXPECT_TRUE((mapIn.getPosition().array() == mapOut.getPosition().array()).all());
55  EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all());
56 }
57 
58 TEST(RosbagHandling, saveLoad)
59 {
60  string layer = "layer";
61  string pathToBag = "test.bag";
62  string topic = "topic";
63  vector<string> layers;
64  layers.push_back(layer);
65  grid_map::GridMap gridMapIn(layers), gridMapOut;
66  gridMapIn.setGeometry(grid_map::Length(1.0, 1.0), 0.5);
67  double a = 1.0;
68  for (grid_map::GridMapIterator iterator(gridMapIn); !iterator.isPastEnd(); ++iterator) {
69  gridMapIn.at(layer, *iterator) = a;
70  a += 1.0;
71  }
72 
73  EXPECT_FALSE(gridMapOut.exists(layer));
74 
75  EXPECT_TRUE(GridMapRosConverter::saveToBag(gridMapIn, pathToBag, topic));
76  EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut));
77 
78  EXPECT_TRUE(gridMapOut.exists(layer));
79 
80  for (GridMapIterator iterator(gridMapIn); !iterator.isPastEnd(); ++iterator) {
81  EXPECT_DOUBLE_EQ(gridMapIn.at(layer, *iterator), gridMapOut.at(layer, *iterator));
82  }
83 }
84 
85 TEST(RosbagHandling, saveLoadWithTime)
86 {
87  string layer = "layer";
88  string pathToBag = "test.bag";
89  string topic = "topic";
90  vector<string> layers;
91  layers.push_back(layer);
92  GridMap gridMapIn(layers), gridMapOut;
93  gridMapIn.setGeometry(grid_map::Length(1.0, 1.0), 0.5);
94  double a = 1.0;
95  for (GridMapIterator iterator(gridMapIn); !iterator.isPastEnd(); ++iterator) {
96  gridMapIn.at(layer, *iterator) = a;
97  a += 1.0;
98  }
99 
100  EXPECT_FALSE(gridMapOut.exists(layer));
101 
103  // TODO Do other time than now.
104  gridMapIn.setTimestamp(ros::Time::now().toNSec());
105 
106  EXPECT_TRUE(GridMapRosConverter::saveToBag(gridMapIn, pathToBag, topic));
107  EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut));
108 
109  EXPECT_TRUE(gridMapOut.exists(layer));
110 
111  for (GridMapIterator iterator(gridMapIn); !iterator.isPastEnd(); ++iterator) {
112  EXPECT_DOUBLE_EQ(gridMapIn.at(layer, *iterator), gridMapOut.at(layer, *iterator));
113  }
114 }
115 
116 TEST(OccupancyGridConversion, withMove)
117 {
118  grid_map::GridMap map;
119  map.setGeometry(grid_map::Length(8.0, 5.0), 0.5, grid_map::Position(0.0, 0.0));
120  map.add("layer", 1.0);
121 
122  // Convert to OccupancyGrid msg.
123  nav_msgs::OccupancyGrid occupancyGrid;
124  GridMapRosConverter::toOccupancyGrid(map, "layer", 0.0, 1.0, occupancyGrid);
125 
126  // Expect the (0, 0) cell to have value 100.
127  EXPECT_DOUBLE_EQ(100.0, occupancyGrid.data[0]);
128 
129  // Move the map, so the cell (0, 0) will move to unobserved space.
130  map.move(grid_map::Position(-1.0, -1.0));
131 
132  // Convert again to OccupancyGrid msg.
133  nav_msgs::OccupancyGrid occupancyGridNew;
134  GridMapRosConverter::toOccupancyGrid(map, "layer", 0.0, 1.0, occupancyGridNew);
135 
136  // Now the (0, 0) cell should be unobserved (-1).
137  EXPECT_DOUBLE_EQ(-1.0, occupancyGridNew.data[0]);
138 }
139 
140 TEST(OccupancyGridConversion, roundTrip)
141 {
142  // Create occupancy grid.
143  nav_msgs::OccupancyGrid occupancyGrid;
144  occupancyGrid.header.stamp = ros::Time(5.0);
145  occupancyGrid.header.frame_id = "map";
146  occupancyGrid.info.resolution = 0.1;
147  occupancyGrid.info.width = 50;
148  occupancyGrid.info.height = 100;
149  occupancyGrid.info.origin.position.x = 3.0;
150  occupancyGrid.info.origin.position.y = 6.0;
151  occupancyGrid.info.origin.orientation.w = 1.0;
152  occupancyGrid.data.resize(occupancyGrid.info.width * occupancyGrid.info.height);
153 
154  for (auto& cell : occupancyGrid.data) {
155  cell = rand() % 102 - 1; // [-1, 100]
156  }
157 
158  // Convert to grid map.
159  GridMap gridMap;
160  GridMapRosConverter::fromOccupancyGrid(occupancyGrid, "layer", gridMap);
161 
162  // Convert back to occupancy grid.
163  nav_msgs::OccupancyGrid occupancyGridResult;
164  GridMapRosConverter::toOccupancyGrid(gridMap, "layer", -1.0, 100.0, occupancyGridResult);
165 
166  // Check map info.
167  EXPECT_EQ(occupancyGrid.header.stamp, occupancyGridResult.header.stamp);
168  EXPECT_EQ(occupancyGrid.header.frame_id, occupancyGridResult.header.frame_id);
169  EXPECT_EQ(occupancyGrid.info.width, occupancyGridResult.info.width);
170  EXPECT_EQ(occupancyGrid.info.height, occupancyGridResult.info.height);
171  EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.position.x, occupancyGridResult.info.origin.position.x);
172  EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.position.x, occupancyGridResult.info.origin.position.x);
173  EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.x, occupancyGridResult.info.origin.orientation.x);
174  EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.y, occupancyGridResult.info.origin.orientation.y);
175  EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.z, occupancyGridResult.info.origin.orientation.z);
176  EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.w, occupancyGridResult.info.origin.orientation.w);
177 
178  // Check map data.
179  for (std::vector<int8_t>::iterator iterator = occupancyGrid.data.begin();
180  iterator != occupancyGrid.data.end(); ++iterator) {
181  size_t i = std::distance(occupancyGrid.data.begin(), iterator);
182  EXPECT_EQ((int)*iterator, (int)occupancyGridResult.data[i]);
183  }
184 }
185 
186 TEST(ImageConversion, roundTripBGRA8)
187 {
188  // Create grid map.
189  GridMap mapIn({"layer"});
190  mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01);
191  mapIn["layer"].setRandom();
192  const float minValue = -1.0;
193  const float maxValue = 1.0;
194 
195  // Convert to image message.
196  sensor_msgs::Image image;
197  GridMapRosConverter::toImage(mapIn, "layer", sensor_msgs::image_encodings::BGRA8, minValue,
198  maxValue, image);
199 
200  // Convert back to grid map.
201  GridMap mapOut;
202  GridMapRosConverter::initializeFromImage(image, mapIn.getResolution(), mapOut);
203  GridMapRosConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue);
204 
205  // Check data.
206  const float resolution = (maxValue - minValue) / (float) std::numeric_limits<unsigned char>::max();
207  expectNear(mapIn["layer"], mapOut["layer"], resolution, "");
208  EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all());
209  EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all());
210 }
211 
212 TEST(ImageConversion, roundTripMONO16)
213 {
214  // Create grid map.
215  GridMap mapIn({"layer"});
216  mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01);
217  mapIn["layer"].setRandom();
218  const float minValue = -1.0;
219  const float maxValue = 1.0;
220 
221  // Convert to image message.
222  sensor_msgs::Image image;
223  GridMapRosConverter::toImage(mapIn, "layer", sensor_msgs::image_encodings::MONO16,
224  minValue, maxValue, image);
225 
226  // Convert back to grid map.
227  GridMap mapOut;
228  GridMapRosConverter::initializeFromImage(image, mapIn.getResolution(), mapOut);
229  GridMapRosConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue);
230 
231  // Check data.
232  // TODO Why is factor 300 necessary?
233  const float resolution = 300.0 * (maxValue - minValue) / (float) std::numeric_limits<unsigned short>::max();
234  expectNear(mapIn["layer"], mapOut["layer"], resolution, "");
235  EXPECT_EQ(mapIn.getTimestamp(), mapOut.getTimestamp());
236  EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all());
237  EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all());
238 }
const Length & getLength() const
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
Time getTimestamp() const
bool getPosition(const Index &index, Position &position) const
const std::string & getFrameId() const
TEST(RosMessageConversion, roundTrip)
bool move(const Position &position, std::vector< BufferRegion > &newRegions)
bool exists(const std::string &layer) const
const std::vector< std::string > & getLayers() const
void expectNear(const M1 &A, const M2 &B, T tolerance, std::string const &message="")
Eigen::Vector2d Position
static bool isValid()
static void init()
const std::string MONO16
float & at(const std::string &layer, const Index &index)
void add(const std::string &layer, const double value=NAN)
static Time now()
Eigen::Array2d Length
const Size & getSize() const


grid_map_ros
Author(s): P├ęter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:35