GridMapRosConverter.cpp
Go to the documentation of this file.
00001 /*
00002  * GridMapRosConverter.cpp
00003  *
00004  *  Created on: Jul 14, 2014
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich, Autonomous Systems Lab
00007  */
00008 
00009 #include "grid_map_ros/GridMapRosConverter.hpp"
00010 #include "grid_map_ros/GridMapMsgHelpers.hpp"
00011 #include <grid_map_cv/grid_map_cv.hpp>
00012 
00013 // ROS
00014 #include <sensor_msgs/point_cloud2_iterator.h>
00015 #include <geometry_msgs/Point.h>
00016 #include <geometry_msgs/Quaternion.h>
00017 #include <rosbag/bag.h>
00018 #include <rosbag/view.h>
00019 
00020 // STL
00021 #include <limits>
00022 #include <algorithm>
00023 #include <vector>
00024 
00025 using namespace std;
00026 using namespace Eigen;
00027 
00028 namespace grid_map {
00029 
00030 GridMapRosConverter::GridMapRosConverter()
00031 {
00032 }
00033 
00034 GridMapRosConverter::~GridMapRosConverter()
00035 {
00036 }
00037 
00038 bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap)
00039 {
00040   gridMap.setTimestamp(message.info.header.stamp.toNSec());
00041   gridMap.setFrameId(message.info.header.frame_id);
00042   gridMap.setGeometry(Length(message.info.length_x, message.info.length_y), message.info.resolution,
00043                       Position(message.info.pose.position.x, message.info.pose.position.y));
00044 
00045   if (message.layers.size() != message.data.size()) {
00046     ROS_ERROR("Different number of layers and data in grid map message.");
00047     return false;
00048   }
00049 
00050   for (unsigned int i = 0; i < message.layers.size(); i++) {
00051     Matrix data;
00052     multiArrayMessageCopyToMatrixEigen(message.data[i], data); // TODO Could we use the data mapping (instead of copying) method here?
00053     // TODO Check if size is good.   size_ << getRows(message.data[0]), getCols(message.data[0]);
00054     gridMap.add(message.layers[i], data);
00055   }
00056 
00057   gridMap.setBasicLayers(message.basic_layers);
00058   gridMap.setStartIndex(Index(message.outer_start_index, message.inner_start_index));
00059   return true;
00060 }
00061 
00062 void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, grid_map_msgs::GridMap& message)
00063 {
00064   toMessage(gridMap, gridMap.getLayers(), message);
00065 }
00066 
00067 void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, const std::vector<std::string>& layers,
00068                       grid_map_msgs::GridMap& message)
00069 {
00070   message.info.header.stamp.fromNSec(gridMap.getTimestamp());
00071   message.info.header.frame_id = gridMap.getFrameId();
00072   message.info.resolution = gridMap.getResolution();
00073   message.info.length_x = gridMap.getLength().x();
00074   message.info.length_y = gridMap.getLength().y();
00075   message.info.pose.position.x = gridMap.getPosition().x();
00076   message.info.pose.position.y = gridMap.getPosition().y();
00077   message.info.pose.position.z = 0.0;
00078   message.info.pose.orientation.x = 0.0;
00079   message.info.pose.orientation.y = 0.0;
00080   message.info.pose.orientation.z = 0.0;
00081   message.info.pose.orientation.w = 1.0;
00082 
00083   message.layers = layers;
00084   message.basic_layers = gridMap.getBasicLayers();
00085 
00086   message.data.clear();
00087   for (const auto& layer : layers) {
00088     std_msgs::Float32MultiArray dataArray;
00089     matrixEigenCopyToMultiArrayMessage(gridMap.get(layer), dataArray);
00090     message.data.push_back(dataArray);
00091   }
00092 
00093   message.outer_start_index = gridMap.getStartIndex()(0);
00094   message.inner_start_index = gridMap.getStartIndex()(1);
00095 }
00096 
00097 void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap,
00098                                        const std::string& pointLayer,
00099                                        sensor_msgs::PointCloud2& pointCloud)
00100 {
00101   toPointCloud(gridMap, gridMap.getLayers(), pointLayer, pointCloud);
00102 }
00103 
00104 void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap,
00105                                        const std::vector<std::string>& layers,
00106                                        const std::string& pointLayer,
00107                                        sensor_msgs::PointCloud2& pointCloud)
00108 {
00109   // Header.
00110   pointCloud.header.frame_id = gridMap.getFrameId();
00111   pointCloud.header.stamp.fromNSec(gridMap.getTimestamp());
00112   pointCloud.is_dense = false;
00113 
00114   // Fields.
00115   std::vector <std::string> fieldNames;
00116 
00117   for (const auto& layer : layers) {
00118     if (layer == pointLayer) {
00119       fieldNames.push_back("x");
00120       fieldNames.push_back("y");
00121       fieldNames.push_back("z");
00122     } else if (layer == "color") {
00123       fieldNames.push_back("rgb");
00124     } else {
00125       fieldNames.push_back(layer);
00126     }
00127   }
00128 
00129   pointCloud.fields.clear();
00130   pointCloud.fields.reserve(fieldNames.size());
00131   int offset = 0;
00132 
00133   for (auto& name : fieldNames) {
00134     sensor_msgs::PointField pointField;
00135     pointField.name = name;
00136     pointField.count = 1;
00137     pointField.datatype = sensor_msgs::PointField::FLOAT32;
00138     pointField.offset = offset;
00139     pointCloud.fields.push_back(pointField);
00140     offset = offset + pointField.count * 4;  // 4 for sensor_msgs::PointField::FLOAT32
00141   }
00142 
00143   // Resize.
00144   size_t maxNumberOfPoints = gridMap.getSize().prod();
00145   pointCloud.height = 1;
00146   pointCloud.width = maxNumberOfPoints;
00147   pointCloud.point_step = offset;
00148   pointCloud.row_step = pointCloud.width * pointCloud.point_step;
00149   pointCloud.data.resize(pointCloud.height * pointCloud.row_step);
00150 
00151   // Points.
00152   std::unordered_map<std::string, sensor_msgs::PointCloud2Iterator<float>> fieldIterators;
00153   for (auto& name : fieldNames) {
00154     fieldIterators.insert(
00155         std::pair<std::string, sensor_msgs::PointCloud2Iterator<float>>(
00156             name, sensor_msgs::PointCloud2Iterator<float>(pointCloud, name)));
00157   }
00158 
00159   GridMapIterator mapIterator(gridMap);
00160   const bool hasBasicLayers = gridMap.getBasicLayers().size() > 0;
00161 
00162   size_t realNumberOfPoints = 0;
00163   for (size_t i = 0; i < maxNumberOfPoints; ++i) {
00164     if (hasBasicLayers) {
00165       if (!gridMap.isValid(*mapIterator)) {
00166         ++mapIterator;
00167         continue;
00168       }
00169     }
00170 
00171     Position3 position;
00172     if (!gridMap.getPosition3(pointLayer, *mapIterator, position)) {
00173       ++mapIterator;
00174       continue;
00175     }
00176 
00177     for (auto& iterator : fieldIterators) {
00178       if (iterator.first == "x") {
00179         *iterator.second = (float) position.x();
00180       } else if (iterator.first == "y") {
00181         *iterator.second = (float) position.y();
00182       } else if (iterator.first == "z") {
00183         *iterator.second = (float) position.z();
00184       } else if (iterator.first == "rgb") {
00185         *iterator.second = gridMap.at("color", *mapIterator);
00186       } else {
00187         *iterator.second = gridMap.at(iterator.first, *mapIterator);
00188       }
00189     }
00190 
00191     ++mapIterator;
00192     for (auto& iterator : fieldIterators) {
00193       ++iterator.second;
00194     }
00195     ++realNumberOfPoints;
00196   }
00197 
00198   if (realNumberOfPoints != maxNumberOfPoints) {
00199     pointCloud.width = realNumberOfPoints;
00200     pointCloud.row_step = pointCloud.width * pointCloud.point_step;
00201     pointCloud.data.resize(pointCloud.height * pointCloud.row_step);
00202   }
00203 }
00204 
00205 bool GridMapRosConverter::fromOccupancyGrid(const nav_msgs::OccupancyGrid& occupancyGrid,
00206                                             const std::string& layer, grid_map::GridMap& gridMap)
00207 {
00208   const Size size(occupancyGrid.info.width, occupancyGrid.info.height);
00209   const double resolution = occupancyGrid.info.resolution;
00210   const Length length = resolution * size.cast<double>();
00211   const string& frameId = occupancyGrid.header.frame_id;
00212   Position position(occupancyGrid.info.origin.position.x, occupancyGrid.info.origin.position.y);
00213   // Different conventions of center of map.
00214   position += 0.5 * length.matrix();
00215 
00216   const auto& orientation = occupancyGrid.info.origin.orientation;
00217   if (orientation.w != 1.0 && !(orientation.x == 0 && orientation.y == 0
00218       && orientation.z == 0 && orientation.w == 0)) {
00219     ROS_WARN_STREAM("Conversion of occupancy grid: Grid maps do not support orientation.");
00220     ROS_INFO_STREAM("Orientation of occupancy grid: " << endl << occupancyGrid.info.origin.orientation);
00221     return false;
00222   }
00223 
00224   if (size.prod() != occupancyGrid.data.size()) {
00225     ROS_WARN_STREAM("Conversion of occupancy grid: Size of data does not correspond to width * height.");
00226     return false;
00227   }
00228 
00229   // TODO: Split to `initializeFrom` and `from` as for Costmap2d.
00230   if ((gridMap.getSize() != size).any() || gridMap.getResolution() != resolution
00231       || (gridMap.getLength() != length).any() || gridMap.getPosition() != position
00232       || gridMap.getFrameId() != frameId || !gridMap.getStartIndex().isZero()) {
00233     gridMap.setTimestamp(occupancyGrid.header.stamp.toNSec());
00234     gridMap.setFrameId(frameId);
00235     gridMap.setGeometry(length, resolution, position);
00236   }
00237 
00238   // Reverse iteration is required because of different conventions
00239   // between occupancy grid and grid map.
00240   grid_map::Matrix data(size(0), size(1));
00241   for (std::vector<int8_t>::const_reverse_iterator iterator = occupancyGrid.data.rbegin();
00242       iterator != occupancyGrid.data.rend(); ++iterator) {
00243     size_t i = std::distance(occupancyGrid.data.rbegin(), iterator);
00244     data(i) = *iterator != -1 ? *iterator : NAN;
00245   }
00246 
00247   gridMap.add(layer, data);
00248   return true;
00249 }
00250 
00251 void GridMapRosConverter::toOccupancyGrid(const grid_map::GridMap& gridMap,
00252                                           const std::string& layer, float dataMin, float dataMax,
00253                                           nav_msgs::OccupancyGrid& occupancyGrid)
00254 {
00255   occupancyGrid.header.frame_id = gridMap.getFrameId();
00256   occupancyGrid.header.stamp.fromNSec(gridMap.getTimestamp());
00257   occupancyGrid.info.map_load_time = occupancyGrid.header.stamp;  // Same as header stamp as we do not load the map.
00258   occupancyGrid.info.resolution = gridMap.getResolution();
00259   occupancyGrid.info.width = gridMap.getSize()(0);
00260   occupancyGrid.info.height = gridMap.getSize()(1);
00261   Position position = gridMap.getPosition() - 0.5 * gridMap.getLength().matrix();
00262   occupancyGrid.info.origin.position.x = position.x();
00263   occupancyGrid.info.origin.position.y = position.y();
00264   occupancyGrid.info.origin.position.z = 0.0;
00265   occupancyGrid.info.origin.orientation.x = 0.0;
00266   occupancyGrid.info.origin.orientation.y = 0.0;
00267   occupancyGrid.info.origin.orientation.z = 0.0;
00268   occupancyGrid.info.origin.orientation.w = 1.0;
00269   size_t nCells = gridMap.getSize().prod();
00270   occupancyGrid.data.resize(nCells);
00271 
00272   // Occupancy probabilities are in the range [0,100]. Unknown is -1.
00273   const float cellMin = 0;
00274   const float cellMax = 100;
00275   const float cellRange = cellMax - cellMin;
00276 
00277   for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
00278     float value = (gridMap.at(layer, *iterator) - dataMin) / (dataMax - dataMin);
00279     if (isnan(value))
00280       value = -1;
00281     else
00282       value = cellMin + min(max(0.0f, value), 1.0f) * cellRange;
00283     size_t index = getLinearIndexFromIndex(iterator.getUnwrappedIndex(), gridMap.getSize(), false);
00284     // Reverse cell order because of different conventions between occupancy grid and grid map.
00285     occupancyGrid.data[nCells - index - 1] = value;
00286   }
00287 }
00288 
00289 void GridMapRosConverter::toGridCells(const grid_map::GridMap& gridMap, const std::string& layer,
00290                                       float lowerThreshold, float upperThreshold,
00291                                       nav_msgs::GridCells& gridCells)
00292 {
00293   gridCells.header.frame_id = gridMap.getFrameId();
00294   gridCells.header.stamp.fromNSec(gridMap.getTimestamp());
00295   gridCells.cell_width = gridMap.getResolution();
00296   gridCells.cell_height = gridMap.getResolution();
00297 
00298   for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
00299     if (!gridMap.isValid(*iterator, layer)) continue;
00300     if (gridMap.at(layer, *iterator) >= lowerThreshold
00301         && gridMap.at(layer, *iterator) <= upperThreshold) {
00302       Position position;
00303       gridMap.getPosition(*iterator, position);
00304       geometry_msgs::Point point;
00305       point.x = position.x();
00306       point.y = position.y();
00307       gridCells.cells.push_back(point);
00308     }
00309   }
00310 }
00311 
00312 bool GridMapRosConverter::initializeFromImage(const sensor_msgs::Image& image,
00313                                               const double resolution, grid_map::GridMap& gridMap,
00314                                               const grid_map::Position& position)
00315 {
00316   const double lengthX = resolution * image.height;
00317   const double lengthY = resolution * image.width;
00318   Length length(lengthX, lengthY);
00319   gridMap.setGeometry(length, resolution, position);
00320   gridMap.setFrameId(image.header.frame_id);
00321   gridMap.setTimestamp(image.header.stamp.toNSec());
00322   return true;
00323 }
00324 
00325 bool GridMapRosConverter::addLayerFromImage(const sensor_msgs::Image& image,
00326                                             const std::string& layer, grid_map::GridMap& gridMap,
00327                                             const float lowerValue, const float upperValue,
00328                                             const double alphaThreshold)
00329 {
00330   cv_bridge::CvImageConstPtr cvImage;
00331   try {
00332     // TODO Use `toCvShared()`?
00333     cvImage = cv_bridge::toCvCopy(image, image.encoding);
00334   } catch (cv_bridge::Exception& e) {
00335     ROS_ERROR("cv_bridge exception: %s", e.what());
00336     return false;
00337   }
00338 
00339   const int cvEncoding = cv_bridge::getCvType(image.encoding);
00340   switch (cvEncoding) {
00341     case CV_8UC1:
00342       return GridMapCvConverter::addLayerFromImage<unsigned char, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
00343     case CV_8UC3:
00344       return GridMapCvConverter::addLayerFromImage<unsigned char, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
00345     case CV_8UC4:
00346       return GridMapCvConverter::addLayerFromImage<unsigned char, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
00347     case CV_16UC1:
00348       return GridMapCvConverter::addLayerFromImage<unsigned short, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
00349     case CV_16UC3:
00350       return GridMapCvConverter::addLayerFromImage<unsigned short, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
00351     case CV_16UC4:
00352       return GridMapCvConverter::addLayerFromImage<unsigned short, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
00353     default:
00354       ROS_ERROR("Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding.");
00355       return false;
00356   }
00357 }
00358 
00359 bool GridMapRosConverter::addColorLayerFromImage(const sensor_msgs::Image& image,
00360                                                  const std::string& layer,
00361                                                  grid_map::GridMap& gridMap)
00362 {
00363   cv_bridge::CvImageConstPtr cvImage;
00364   try {
00365     cvImage = cv_bridge::toCvCopy(image, image.encoding);
00366   } catch (cv_bridge::Exception& e) {
00367     ROS_ERROR("cv_bridge exception: %s", e.what());
00368     return false;
00369   }
00370 
00371   const int cvEncoding = cv_bridge::getCvType(image.encoding);
00372   switch (cvEncoding) {
00373     case CV_8UC3:
00374       return GridMapCvConverter::addColorLayerFromImage<unsigned char, 3>(cvImage->image, layer, gridMap);
00375     case CV_8UC4:
00376       return GridMapCvConverter::addColorLayerFromImage<unsigned char, 4>(cvImage->image, layer, gridMap);
00377     case CV_16UC3:
00378       return GridMapCvConverter::addColorLayerFromImage<unsigned short, 3>(cvImage->image, layer, gridMap);
00379     case CV_16UC4:
00380       return GridMapCvConverter::addColorLayerFromImage<unsigned short, 4>(cvImage->image, layer, gridMap);
00381     default:
00382       ROS_ERROR("Expected RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding.");
00383       return false;
00384   }
00385 }
00386 
00387 bool GridMapRosConverter::toImage(const grid_map::GridMap& gridMap, const std::string& layer,
00388                                   const std::string encoding, sensor_msgs::Image& image)
00389 {
00390   cv_bridge::CvImage cvImage;
00391   if (!toCvImage(gridMap, layer, encoding, cvImage)) return false;
00392   cvImage.toImageMsg(image);
00393   return true;
00394 }
00395 
00396 bool GridMapRosConverter::toImage(const grid_map::GridMap& gridMap, const std::string& layer,
00397                                   const std::string encoding, const float lowerValue,
00398                                   const float upperValue, sensor_msgs::Image& image)
00399 {
00400   cv_bridge::CvImage cvImage;
00401   if (!toCvImage(gridMap, layer, encoding, lowerValue, upperValue, cvImage)) return false;
00402   cvImage.toImageMsg(image);
00403   return true;
00404 }
00405 
00406 bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
00407                                     const std::string encoding, cv_bridge::CvImage& cvImage)
00408 {
00409   const float minValue = gridMap.get(layer).minCoeffOfFinites();
00410   const float maxValue = gridMap.get(layer).maxCoeffOfFinites();
00411   return toCvImage(gridMap, layer, encoding, minValue, maxValue, cvImage);
00412 }
00413 
00414 bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
00415                                     const std::string encoding, const float lowerValue,
00416                                     const float upperValue, cv_bridge::CvImage& cvImage)
00417 {
00418   cvImage.header.stamp.fromNSec(gridMap.getTimestamp());
00419   cvImage.header.frame_id = gridMap.getFrameId();
00420   cvImage.encoding = encoding;
00421 
00422   const int cvEncoding = cv_bridge::getCvType(encoding);
00423   switch (cvEncoding) {
00424     case CV_8UC1:
00425       return GridMapCvConverter::toImage<unsigned char, 1>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
00426     case CV_8UC3:
00427       return GridMapCvConverter::toImage<unsigned char, 3>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
00428     case CV_8UC4:
00429       return GridMapCvConverter::toImage<unsigned char, 4>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
00430     case CV_16UC1:
00431       return GridMapCvConverter::toImage<unsigned short, 1>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
00432     case CV_16UC3:
00433       return GridMapCvConverter::toImage<unsigned short, 3>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
00434     case CV_16UC4:
00435       return GridMapCvConverter::toImage<unsigned short, 4>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
00436     default:
00437       ROS_ERROR("Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding.");
00438       return false;
00439   }
00440 }
00441 
00442 bool GridMapRosConverter::saveToBag(const grid_map::GridMap& gridMap, const std::string& pathToBag,
00443                                     const std::string& topic)
00444 {
00445   grid_map_msgs::GridMap message;
00446   toMessage(gridMap, message);
00447   ros::Time time(gridMap.getTimestamp());
00448 
00449   if (!time.isValid() || time.isZero()) {
00450     if (!ros::Time::isValid()) ros::Time::init();
00451     time = ros::Time::now();
00452   }
00453 
00454   rosbag::Bag bag;
00455   bag.open(pathToBag, rosbag::bagmode::Write);
00456   bag.write(topic, time, message);
00457   bag.close();
00458   return true;
00459 }
00460 
00461 bool GridMapRosConverter::loadFromBag(const std::string& pathToBag, const std::string& topic,
00462                                       grid_map::GridMap& gridMap)
00463 {
00464   rosbag::Bag bag;
00465   bag.open(pathToBag, rosbag::bagmode::Read);
00466   rosbag::View view(bag, rosbag::TopicQuery(topic));
00467 
00468   bool isDataFound = false;
00469   for (const auto& messageInstance : view) {
00470     grid_map_msgs::GridMap::ConstPtr message = messageInstance.instantiate<grid_map_msgs::GridMap>();
00471     if (message != NULL) {
00472       fromMessage(*message, gridMap);
00473       isDataFound = true;
00474     } else {
00475       bag.close();
00476       ROS_WARN("Unable to load data from ROS bag.");
00477       return false;
00478     }
00479   }
00480   if (!isDataFound) ROS_WARN_STREAM("No data under the topic '" << topic << "' was found.");
00481   bag.close();
00482   return true;
00483 }
00484 
00485 } /* namespace */


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