GridMapRosTest.cpp
Go to the documentation of this file.
00001 /*
00002  * GridMapRosTest.cpp
00003  *
00004  *  Created on: Mar 24, 2015
00005  *      Author: Peter Fankhauser, Martin Wermelinger
00006  *       Institute: ETH Zurich, Autonomous Systems Lab
00007  */
00008 
00009 #include "grid_map_core/GridMap.hpp"
00010 #include "grid_map_core/iterators/GridMapIterator.hpp"
00011 #include "grid_map_core/gtest_eigen.hpp"
00012 #include "grid_map_ros/GridMapRosConverter.hpp"
00013 #include "grid_map_msgs/GridMap.h"
00014 
00015 // gtest
00016 #include <gtest/gtest.h>
00017 
00018 // Eigen
00019 #include <Eigen/Core>
00020 
00021 // STD
00022 #include <string>
00023 #include <vector>
00024 #include <stdlib.h>
00025 #include <iterator>
00026 
00027 // ROS
00028 #include <nav_msgs/OccupancyGrid.h>
00029 #include <cv_bridge/cv_bridge.h>
00030 #include <sensor_msgs/image_encodings.h>
00031 
00032 using namespace std;
00033 using namespace grid_map;
00034 
00035 TEST(RosMessageConversion, roundTrip)
00036 {
00037   GridMap mapIn({"layer"});
00038   mapIn.setGeometry(Length(2.0, 3.0), 0.5, Position(1.0, 1.5));
00039   mapIn["layer"].setRandom();
00040 
00041   grid_map_msgs::GridMap message;
00042   GridMapRosConverter::toMessage(mapIn, message);
00043   GridMap mapOut;
00044   GridMapRosConverter::fromMessage(message, mapOut);
00045 
00046   for (size_t i = 0; i < mapIn.getLayers().size(); ++i) {
00047     EXPECT_EQ(mapIn.getLayers().at(i), mapOut.getLayers().at(i));
00048     const std::string layer = mapIn.getLayers().at(i);
00049     EXPECT_TRUE((mapIn[layer].array() == mapOut[layer].array()).all());
00050   }
00051 
00052   EXPECT_EQ(mapIn.getFrameId(), mapOut.getFrameId());
00053   EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all());
00054   EXPECT_TRUE((mapIn.getPosition().array() == mapOut.getPosition().array()).all());
00055   EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all());
00056 }
00057 
00058 TEST(RosbagHandling, saveLoad)
00059 {
00060   string layer = "layer";
00061   string pathToBag = "test.bag";
00062   string topic = "topic";
00063   vector<string> layers;
00064   layers.push_back(layer);
00065   grid_map::GridMap gridMapIn(layers), gridMapOut;
00066   gridMapIn.setGeometry(grid_map::Length(1.0, 1.0), 0.5);
00067   double a = 1.0;
00068   for (grid_map::GridMapIterator iterator(gridMapIn); !iterator.isPastEnd(); ++iterator) {
00069     gridMapIn.at(layer, *iterator) = a;
00070     a += 1.0;
00071   }
00072 
00073   EXPECT_FALSE(gridMapOut.exists(layer));
00074 
00075   EXPECT_TRUE(GridMapRosConverter::saveToBag(gridMapIn, pathToBag, topic));
00076   EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut));
00077 
00078   EXPECT_TRUE(gridMapOut.exists(layer));
00079 
00080   for (GridMapIterator iterator(gridMapIn); !iterator.isPastEnd(); ++iterator) {
00081     EXPECT_DOUBLE_EQ(gridMapIn.at(layer, *iterator), gridMapOut.at(layer, *iterator));
00082   }
00083 }
00084 
00085 TEST(RosbagHandling, saveLoadWithTime)
00086 {
00087   string layer = "layer";
00088   string pathToBag = "test.bag";
00089   string topic = "topic";
00090   vector<string> layers;
00091   layers.push_back(layer);
00092   GridMap gridMapIn(layers), gridMapOut;
00093   gridMapIn.setGeometry(grid_map::Length(1.0, 1.0), 0.5);
00094   double a = 1.0;
00095   for (GridMapIterator iterator(gridMapIn); !iterator.isPastEnd(); ++iterator) {
00096     gridMapIn.at(layer, *iterator) = a;
00097     a += 1.0;
00098   }
00099 
00100   EXPECT_FALSE(gridMapOut.exists(layer));
00101 
00102   if (!ros::Time::isValid()) ros::Time::init();
00103   // TODO Do other time than now.
00104   gridMapIn.setTimestamp(ros::Time::now().toNSec());
00105 
00106   EXPECT_TRUE(GridMapRosConverter::saveToBag(gridMapIn, pathToBag, topic));
00107   EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut));
00108 
00109   EXPECT_TRUE(gridMapOut.exists(layer));
00110 
00111   for (GridMapIterator iterator(gridMapIn); !iterator.isPastEnd(); ++iterator) {
00112     EXPECT_DOUBLE_EQ(gridMapIn.at(layer, *iterator), gridMapOut.at(layer, *iterator));
00113   }
00114 }
00115 
00116 TEST(OccupancyGridConversion, withMove)
00117 {
00118   grid_map::GridMap map;
00119   map.setGeometry(grid_map::Length(8.0, 5.0), 0.5, grid_map::Position(0.0, 0.0));
00120   map.add("layer", 1.0);
00121 
00122   // Convert to OccupancyGrid msg.
00123   nav_msgs::OccupancyGrid occupancyGrid;
00124   GridMapRosConverter::toOccupancyGrid(map, "layer", 0.0, 1.0, occupancyGrid);
00125 
00126   // Expect the (0, 0) cell to have value 100.
00127   EXPECT_DOUBLE_EQ(100.0, occupancyGrid.data[0]);
00128 
00129   // Move the map, so the cell (0, 0) will move to unobserved space.
00130   map.move(grid_map::Position(-1.0, -1.0));
00131 
00132   // Convert again to OccupancyGrid msg.
00133   nav_msgs::OccupancyGrid occupancyGridNew;
00134   GridMapRosConverter::toOccupancyGrid(map, "layer", 0.0, 1.0, occupancyGridNew);
00135 
00136   // Now the (0, 0) cell should be unobserved (-1).
00137   EXPECT_DOUBLE_EQ(-1.0, occupancyGridNew.data[0]);
00138 }
00139 
00140 TEST(OccupancyGridConversion, roundTrip)
00141 {
00142   // Create occupancy grid.
00143   nav_msgs::OccupancyGrid occupancyGrid;
00144   occupancyGrid.header.stamp = ros::Time(5.0);
00145   occupancyGrid.header.frame_id = "map";
00146   occupancyGrid.info.resolution = 0.1;
00147   occupancyGrid.info.width = 50;
00148   occupancyGrid.info.height = 100;
00149   occupancyGrid.info.origin.position.x = 3.0;
00150   occupancyGrid.info.origin.position.y = 6.0;
00151   occupancyGrid.info.origin.orientation.w = 1.0;
00152   occupancyGrid.data.resize(occupancyGrid.info.width * occupancyGrid.info.height);
00153 
00154   for (auto& cell : occupancyGrid.data) {
00155     cell = rand() % 102 - 1; // [-1, 100]
00156   }
00157 
00158   // Convert to grid map.
00159   GridMap gridMap;
00160   GridMapRosConverter::fromOccupancyGrid(occupancyGrid, "layer", gridMap);
00161 
00162   // Convert back to occupancy grid.
00163   nav_msgs::OccupancyGrid occupancyGridResult;
00164   GridMapRosConverter::toOccupancyGrid(gridMap, "layer", -1.0, 100.0, occupancyGridResult);
00165 
00166   // Check map info.
00167   EXPECT_EQ(occupancyGrid.header.stamp, occupancyGridResult.header.stamp);
00168   EXPECT_EQ(occupancyGrid.header.frame_id, occupancyGridResult.header.frame_id);
00169   EXPECT_EQ(occupancyGrid.info.width, occupancyGridResult.info.width);
00170   EXPECT_EQ(occupancyGrid.info.height, occupancyGridResult.info.height);
00171   EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.position.x, occupancyGridResult.info.origin.position.x);
00172   EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.position.x, occupancyGridResult.info.origin.position.x);
00173   EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.x, occupancyGridResult.info.origin.orientation.x);
00174   EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.y, occupancyGridResult.info.origin.orientation.y);
00175   EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.z, occupancyGridResult.info.origin.orientation.z);
00176   EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.w, occupancyGridResult.info.origin.orientation.w);
00177 
00178   // Check map data.
00179   for (std::vector<int8_t>::iterator iterator = occupancyGrid.data.begin();
00180       iterator != occupancyGrid.data.end(); ++iterator) {
00181     size_t i = std::distance(occupancyGrid.data.begin(), iterator);
00182     EXPECT_EQ((int)*iterator, (int)occupancyGridResult.data[i]);
00183   }
00184 }
00185 
00186 TEST(ImageConversion, roundTripBGRA8)
00187 {
00188   // Create grid map.
00189   GridMap mapIn({"layer"});
00190   mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01);
00191   mapIn["layer"].setRandom();
00192   const float minValue = -1.0;
00193   const float maxValue = 1.0;
00194 
00195   // Convert to image message.
00196   sensor_msgs::Image image;
00197   GridMapRosConverter::toImage(mapIn, "layer", sensor_msgs::image_encodings::BGRA8, minValue,
00198                                maxValue, image);
00199 
00200   // Convert back to grid map.
00201   GridMap mapOut;
00202   GridMapRosConverter::initializeFromImage(image, mapIn.getResolution(), mapOut);
00203   GridMapRosConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue);
00204 
00205   // Check data.
00206   const float resolution = (maxValue - minValue) / (float) std::numeric_limits<unsigned char>::max();
00207   expectNear(mapIn["layer"], mapOut["layer"], resolution, "");
00208   EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all());
00209   EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all());
00210 }
00211 
00212 TEST(ImageConversion, roundTripMONO16)
00213 {
00214   // Create grid map.
00215   GridMap mapIn({"layer"});
00216   mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01);
00217   mapIn["layer"].setRandom();
00218   const float minValue = -1.0;
00219   const float maxValue = 1.0;
00220 
00221   // Convert to image message.
00222   sensor_msgs::Image image;
00223   GridMapRosConverter::toImage(mapIn, "layer", sensor_msgs::image_encodings::MONO16,
00224                                minValue, maxValue, image);
00225 
00226   // Convert back to grid map.
00227   GridMap mapOut;
00228   GridMapRosConverter::initializeFromImage(image, mapIn.getResolution(), mapOut);
00229   GridMapRosConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue);
00230 
00231   // Check data.
00232   // TODO Why is factor 300 necessary?
00233   const float resolution = 300.0 * (maxValue - minValue) / (float) std::numeric_limits<unsigned short>::max();
00234   expectNear(mapIn["layer"], mapOut["layer"], resolution, "");
00235   EXPECT_EQ(mapIn.getTimestamp(), mapOut.getTimestamp());
00236   EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all());
00237   EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all());
00238 }


grid_map_ros
Author(s): Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:29