13 #include "grid_map_msgs/GridMap.h" 16 #include <gtest/gtest.h> 28 #include <nav_msgs/OccupancyGrid.h> 35 TEST(RosMessageConversion, roundTrip)
39 mapIn[
"layer"].setRandom();
41 grid_map_msgs::GridMap message;
42 GridMapRosConverter::toMessage(mapIn, message);
44 GridMapRosConverter::fromMessage(message, mapOut);
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());
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());
58 TEST(RosbagHandling, saveLoad)
60 string layer =
"layer";
61 string pathToBag =
"test.bag";
62 string topic =
"topic";
63 vector<string> layers;
64 layers.push_back(layer);
69 gridMapIn.at(layer, *iterator) = a;
73 EXPECT_FALSE(gridMapOut.
exists(layer));
75 EXPECT_TRUE(GridMapRosConverter::saveToBag(gridMapIn, pathToBag, topic));
76 EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut));
78 EXPECT_TRUE(gridMapOut.
exists(layer));
81 EXPECT_DOUBLE_EQ(gridMapIn.at(layer, *iterator), gridMapOut.
at(layer, *iterator));
85 TEST(RosbagHandling, saveLoadWithTime)
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;
96 gridMapIn.at(layer, *iterator) = a;
100 EXPECT_FALSE(gridMapOut.
exists(layer));
106 EXPECT_TRUE(GridMapRosConverter::saveToBag(gridMapIn, pathToBag, topic));
107 EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut));
109 EXPECT_TRUE(gridMapOut.
exists(layer));
112 EXPECT_DOUBLE_EQ(gridMapIn.at(layer, *iterator), gridMapOut.
at(layer, *iterator));
116 TEST(OccupancyGridConversion, withMove)
120 map.
add(
"layer", 1.0);
123 nav_msgs::OccupancyGrid occupancyGrid;
124 GridMapRosConverter::toOccupancyGrid(map,
"layer", 0.0, 1.0, occupancyGrid);
127 EXPECT_DOUBLE_EQ(100.0, occupancyGrid.data[0]);
133 nav_msgs::OccupancyGrid occupancyGridNew;
134 GridMapRosConverter::toOccupancyGrid(map,
"layer", 0.0, 1.0, occupancyGridNew);
137 EXPECT_DOUBLE_EQ(-1.0, occupancyGridNew.data[0]);
140 TEST(OccupancyGridConversion, roundTrip)
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);
154 for (
auto& cell : occupancyGrid.data) {
155 cell = rand() % 102 - 1;
160 GridMapRosConverter::fromOccupancyGrid(occupancyGrid,
"layer", gridMap);
163 nav_msgs::OccupancyGrid occupancyGridResult;
164 GridMapRosConverter::toOccupancyGrid(gridMap,
"layer", -1.0, 100.0, occupancyGridResult);
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);
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]);
186 TEST(ImageConversion, roundTripBGRA8)
191 mapIn[
"layer"].setRandom();
192 const float minValue = -1.0;
193 const float maxValue = 1.0;
196 sensor_msgs::Image image;
202 GridMapRosConverter::initializeFromImage(image, mapIn.getResolution(), mapOut);
203 GridMapRosConverter::addLayerFromImage(image,
"layer", mapOut, minValue, maxValue);
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());
212 TEST(ImageConversion, roundTripMONO16)
217 mapIn[
"layer"].setRandom();
218 const float minValue = -1.0;
219 const float maxValue = 1.0;
222 sensor_msgs::Image image;
224 minValue, maxValue, image);
228 GridMapRosConverter::initializeFromImage(image, mapIn.getResolution(), mapOut);
229 GridMapRosConverter::addLayerFromImage(image,
"layer", mapOut, minValue, maxValue);
233 const float resolution = 300.0 * (maxValue - minValue) / (
float) std::numeric_limits<unsigned short>::max();
234 expectNear(mapIn[
"layer"], mapOut[
"layer"], resolution,
"");
236 EXPECT_TRUE((mapIn.getLength() == mapOut.
getLength()).all());
237 EXPECT_TRUE((mapIn.getSize() == mapOut.
getSize()).all());
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="")
float & at(const std::string &layer, const Index &index)
void add(const std::string &layer, const double value=NAN)
const Size & getSize() const