GridMapRosConverter.cpp
Go to the documentation of this file.
1 /*
2  * GridMapRosConverter.cpp
3  *
4  * Created on: Jul 14, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
12 
13 // ROS
15 #include <geometry_msgs/Point.h>
16 #include <geometry_msgs/Quaternion.h>
17 #include <rosbag/bag.h>
18 #include <rosbag/view.h>
19 
20 // STL
21 #include <limits>
22 #include <algorithm>
23 #include <vector>
24 
25 using namespace std;
26 using namespace Eigen;
27 
28 namespace grid_map {
29 
30 GridMapRosConverter::GridMapRosConverter()
31 {
32 }
33 
34 GridMapRosConverter::~GridMapRosConverter()
35 {
36 }
37 
38 bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap, const std::vector<std::string>& layers, bool copyBasicLayers, bool copyAllNonBasicLayers)
39 {
40  gridMap.setTimestamp(message.info.header.stamp.toNSec());
41  gridMap.setFrameId(message.info.header.frame_id);
42  gridMap.setGeometry(Length(message.info.length_x, message.info.length_y), message.info.resolution,
43  Position(message.info.pose.position.x, message.info.pose.position.y));
44 
45  if (message.layers.size() != message.data.size()) {
46  ROS_ERROR("Different number of layers and data in grid map message.");
47  return false;
48  }
49 
50  // Copy non-basic layers.
51  for (unsigned int i = 0u; i < message.layers.size(); ++i) {
52 
53  // check if layer should be copied.
54  if (!copyAllNonBasicLayers && std::find(layers.begin(), layers.end(), message.layers[i]) == layers.end()) {
55  continue;
56  }
57 
58  // TODO Could we use the data mapping (instead of copying) method here?
59  Matrix data;
60  if(!multiArrayMessageCopyToMatrixEigen(message.data[i], data)) {
61  return false;
62  }
63 
64  // TODO Check if size is good. size_ << getRows(message.data[0]), getCols(message.data[0]);
65  gridMap.add(message.layers[i], data);
66  }
67 
68  // Copy basic layers.
69  if (copyBasicLayers) {
70  gridMap.setBasicLayers(message.basic_layers);
71  }
72 
73  gridMap.setStartIndex(Index(message.outer_start_index, message.inner_start_index));
74  return true;
75 }
76 
77 bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap)
78 {
79  return fromMessage(message, gridMap, std::vector<std::string>(), true, true);
80 }
81 
82 void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, grid_map_msgs::GridMap& message)
83 {
84  toMessage(gridMap, gridMap.getLayers(), message);
85 }
86 
87 void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, const std::vector<std::string>& layers,
88  grid_map_msgs::GridMap& message)
89 {
90  message.info.header.stamp.fromNSec(gridMap.getTimestamp());
91  message.info.header.frame_id = gridMap.getFrameId();
92  message.info.resolution = gridMap.getResolution();
93  message.info.length_x = gridMap.getLength().x();
94  message.info.length_y = gridMap.getLength().y();
95  message.info.pose.position.x = gridMap.getPosition().x();
96  message.info.pose.position.y = gridMap.getPosition().y();
97  message.info.pose.position.z = 0.0;
98  message.info.pose.orientation.x = 0.0;
99  message.info.pose.orientation.y = 0.0;
100  message.info.pose.orientation.z = 0.0;
101  message.info.pose.orientation.w = 1.0;
102 
103  message.layers = layers;
104  message.basic_layers = gridMap.getBasicLayers();
105 
106  message.data.clear();
107  for (const auto& layer : layers) {
108  std_msgs::Float32MultiArray dataArray;
109  matrixEigenCopyToMultiArrayMessage(gridMap.get(layer), dataArray);
110  message.data.push_back(dataArray);
111  }
112 
113  message.outer_start_index = gridMap.getStartIndex()(0);
114  message.inner_start_index = gridMap.getStartIndex()(1);
115 }
116 
117 void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap,
118  const std::string& pointLayer,
119  sensor_msgs::PointCloud2& pointCloud)
120 {
121  toPointCloud(gridMap, gridMap.getLayers(), pointLayer, pointCloud);
122 }
123 
124 void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap,
125  const std::vector<std::string>& layers,
126  const std::string& pointLayer,
127  sensor_msgs::PointCloud2& pointCloud)
128 {
129  // Header.
130  pointCloud.header.frame_id = gridMap.getFrameId();
131  pointCloud.header.stamp.fromNSec(gridMap.getTimestamp());
132  pointCloud.is_dense = false;
133 
134  // Fields.
135  std::vector <std::string> fieldNames;
136 
137  for (const auto& layer : layers) {
138  if (layer == pointLayer) {
139  fieldNames.push_back("x");
140  fieldNames.push_back("y");
141  fieldNames.push_back("z");
142  } else if (layer == "color") {
143  fieldNames.push_back("rgb");
144  } else {
145  fieldNames.push_back(layer);
146  }
147  }
148 
149  pointCloud.fields.clear();
150  pointCloud.fields.reserve(fieldNames.size());
151  int offset = 0;
152 
153  for (auto& name : fieldNames) {
154  sensor_msgs::PointField pointField;
155  pointField.name = name;
156  pointField.count = 1;
157  pointField.datatype = sensor_msgs::PointField::FLOAT32;
158  pointField.offset = offset;
159  pointCloud.fields.push_back(pointField);
160  offset = offset + pointField.count * 4; // 4 for sensor_msgs::PointField::FLOAT32
161  }
162 
163  // Resize.
164  size_t maxNumberOfPoints = gridMap.getSize().prod();
165  pointCloud.height = 1;
166  pointCloud.width = maxNumberOfPoints;
167  pointCloud.point_step = offset;
168  pointCloud.row_step = pointCloud.width * pointCloud.point_step;
169  pointCloud.data.resize(pointCloud.height * pointCloud.row_step);
170 
171  // Points.
172  std::unordered_map<std::string, sensor_msgs::PointCloud2Iterator<float>> fieldIterators;
173  for (auto& name : fieldNames) {
174  fieldIterators.insert(
175  std::pair<std::string, sensor_msgs::PointCloud2Iterator<float>>(
176  name, sensor_msgs::PointCloud2Iterator<float>(pointCloud, name)));
177  }
178 
179  GridMapIterator mapIterator(gridMap);
180  const bool hasBasicLayers = gridMap.getBasicLayers().size() > 0;
181 
182  size_t realNumberOfPoints = 0;
183  for (size_t i = 0; i < maxNumberOfPoints; ++i) {
184  if (hasBasicLayers) {
185  if (!gridMap.isValid(*mapIterator)) {
186  ++mapIterator;
187  continue;
188  }
189  }
190 
191  Position3 position;
192  if (!gridMap.getPosition3(pointLayer, *mapIterator, position)) {
193  ++mapIterator;
194  continue;
195  }
196 
197  for (auto& iterator : fieldIterators) {
198  if (iterator.first == "x") {
199  *iterator.second = (float) position.x();
200  } else if (iterator.first == "y") {
201  *iterator.second = (float) position.y();
202  } else if (iterator.first == "z") {
203  *iterator.second = (float) position.z();
204  } else if (iterator.first == "rgb") {
205  *iterator.second = gridMap.at("color", *mapIterator);
206  } else {
207  *iterator.second = gridMap.at(iterator.first, *mapIterator);
208  }
209  }
210 
211  ++mapIterator;
212  for (auto& iterator : fieldIterators) {
213  ++iterator.second;
214  }
215  ++realNumberOfPoints;
216  }
217 
218  if (realNumberOfPoints != maxNumberOfPoints) {
219  pointCloud.width = realNumberOfPoints;
220  pointCloud.row_step = pointCloud.width * pointCloud.point_step;
221  pointCloud.data.resize(pointCloud.height * pointCloud.row_step);
222  }
223 }
224 
225 bool GridMapRosConverter::fromOccupancyGrid(const nav_msgs::OccupancyGrid& occupancyGrid,
226  const std::string& layer, grid_map::GridMap& gridMap)
227 {
228  const Size size(occupancyGrid.info.width, occupancyGrid.info.height);
229  const double resolution = occupancyGrid.info.resolution;
230  const Length length = resolution * size.cast<double>();
231  const string& frameId = occupancyGrid.header.frame_id;
232  Position position(occupancyGrid.info.origin.position.x, occupancyGrid.info.origin.position.y);
233  // Different conventions of center of map.
234  position += 0.5 * length.matrix();
235 
236  const auto& orientation = occupancyGrid.info.origin.orientation;
237  if (orientation.w != 1.0 && !(orientation.x == 0 && orientation.y == 0
238  && orientation.z == 0 && orientation.w == 0)) {
239  ROS_WARN_STREAM("Conversion of occupancy grid: Grid maps do not support orientation.");
240  ROS_INFO_STREAM("Orientation of occupancy grid: " << endl << occupancyGrid.info.origin.orientation);
241  return false;
242  }
243 
244  if (static_cast<size_t>(size.prod()) != occupancyGrid.data.size()) {
245  ROS_WARN_STREAM("Conversion of occupancy grid: Size of data does not correspond to width * height.");
246  return false;
247  }
248 
249  // TODO: Split to `initializeFrom` and `from` as for Costmap2d.
250  if ((gridMap.getSize() != size).any() || gridMap.getResolution() != resolution
251  || (gridMap.getLength() != length).any() || gridMap.getPosition() != position
252  || gridMap.getFrameId() != frameId || !gridMap.getStartIndex().isZero()) {
253  gridMap.setTimestamp(occupancyGrid.header.stamp.toNSec());
254  gridMap.setFrameId(frameId);
255  gridMap.setGeometry(length, resolution, position);
256  }
257 
258  // Reverse iteration is required because of different conventions
259  // between occupancy grid and grid map.
260  grid_map::Matrix data(size(0), size(1));
261  for (std::vector<int8_t>::const_reverse_iterator iterator = occupancyGrid.data.rbegin();
262  iterator != occupancyGrid.data.rend(); ++iterator) {
263  size_t i = std::distance(occupancyGrid.data.rbegin(), iterator);
264  data(i) = *iterator != -1 ? *iterator : NAN;
265  }
266 
267  gridMap.add(layer, data);
268  return true;
269 }
270 
271 void GridMapRosConverter::toOccupancyGrid(const grid_map::GridMap& gridMap,
272  const std::string& layer, float dataMin, float dataMax,
273  nav_msgs::OccupancyGrid& occupancyGrid)
274 {
275  occupancyGrid.header.frame_id = gridMap.getFrameId();
276  occupancyGrid.header.stamp.fromNSec(gridMap.getTimestamp());
277  occupancyGrid.info.map_load_time = occupancyGrid.header.stamp; // Same as header stamp as we do not load the map.
278  occupancyGrid.info.resolution = gridMap.getResolution();
279  occupancyGrid.info.width = gridMap.getSize()(0);
280  occupancyGrid.info.height = gridMap.getSize()(1);
281  Position position = gridMap.getPosition() - 0.5 * gridMap.getLength().matrix();
282  occupancyGrid.info.origin.position.x = position.x();
283  occupancyGrid.info.origin.position.y = position.y();
284  occupancyGrid.info.origin.position.z = 0.0;
285  occupancyGrid.info.origin.orientation.x = 0.0;
286  occupancyGrid.info.origin.orientation.y = 0.0;
287  occupancyGrid.info.origin.orientation.z = 0.0;
288  occupancyGrid.info.origin.orientation.w = 1.0;
289  size_t nCells = gridMap.getSize().prod();
290  occupancyGrid.data.resize(nCells);
291 
292  // Occupancy probabilities are in the range [0,100]. Unknown is -1.
293  const float cellMin = 0;
294  const float cellMax = 100;
295  const float cellRange = cellMax - cellMin;
296 
297  for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
298  float value = (gridMap.at(layer, *iterator) - dataMin) / (dataMax - dataMin);
299  if (isnan(value))
300  value = -1;
301  else
302  value = cellMin + min(max(0.0f, value), 1.0f) * cellRange;
303  size_t index = getLinearIndexFromIndex(iterator.getUnwrappedIndex(), gridMap.getSize(), false);
304  // Reverse cell order because of different conventions between occupancy grid and grid map.
305  occupancyGrid.data[nCells - index - 1] = value;
306  }
307 }
308 
309 void GridMapRosConverter::toGridCells(const grid_map::GridMap& gridMap, const std::string& layer,
310  float lowerThreshold, float upperThreshold,
311  nav_msgs::GridCells& gridCells)
312 {
313  gridCells.header.frame_id = gridMap.getFrameId();
314  gridCells.header.stamp.fromNSec(gridMap.getTimestamp());
315  gridCells.cell_width = gridMap.getResolution();
316  gridCells.cell_height = gridMap.getResolution();
317 
318  for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
319  if (!gridMap.isValid(*iterator, layer)) continue;
320  if (gridMap.at(layer, *iterator) >= lowerThreshold
321  && gridMap.at(layer, *iterator) <= upperThreshold) {
322  Position position;
323  gridMap.getPosition(*iterator, position);
324  geometry_msgs::Point point;
325  point.x = position.x();
326  point.y = position.y();
327  gridCells.cells.push_back(point);
328  }
329  }
330 }
331 
332 bool GridMapRosConverter::initializeFromImage(const sensor_msgs::Image& image,
333  const double resolution, grid_map::GridMap& gridMap,
334  const grid_map::Position& position)
335 {
336  const double lengthX = resolution * image.height;
337  const double lengthY = resolution * image.width;
338  Length length(lengthX, lengthY);
339  gridMap.setGeometry(length, resolution, position);
340  gridMap.setFrameId(image.header.frame_id);
341  gridMap.setTimestamp(image.header.stamp.toNSec());
342  return true;
343 }
344 
345 bool GridMapRosConverter::addLayerFromImage(const sensor_msgs::Image& image,
346  const std::string& layer, grid_map::GridMap& gridMap,
347  const float lowerValue, const float upperValue,
348  const double alphaThreshold)
349 {
351  try {
352  // TODO Use `toCvShared()`?
353  cvImage = cv_bridge::toCvCopy(image, image.encoding);
354  } catch (cv_bridge::Exception& e) {
355  ROS_ERROR("cv_bridge exception: %s", e.what());
356  return false;
357  }
358 
359  const int cvEncoding = cv_bridge::getCvType(image.encoding);
360  switch (cvEncoding) {
361  case CV_8UC1:
362  return GridMapCvConverter::addLayerFromImage<unsigned char, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
363  case CV_8UC3:
364  return GridMapCvConverter::addLayerFromImage<unsigned char, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
365  case CV_8UC4:
366  return GridMapCvConverter::addLayerFromImage<unsigned char, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
367  case CV_16UC1:
368  return GridMapCvConverter::addLayerFromImage<unsigned short, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
369  case CV_16UC3:
370  return GridMapCvConverter::addLayerFromImage<unsigned short, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
371  case CV_16UC4:
372  return GridMapCvConverter::addLayerFromImage<unsigned short, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
373  default:
374  ROS_ERROR("Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding.");
375  return false;
376  }
377 }
378 
379 bool GridMapRosConverter::addColorLayerFromImage(const sensor_msgs::Image& image,
380  const std::string& layer,
381  grid_map::GridMap& gridMap)
382 {
384  try {
385  cvImage = cv_bridge::toCvCopy(image, image.encoding);
386  } catch (cv_bridge::Exception& e) {
387  ROS_ERROR("cv_bridge exception: %s", e.what());
388  return false;
389  }
390 
391  const int cvEncoding = cv_bridge::getCvType(image.encoding);
392  switch (cvEncoding) {
393  case CV_8UC3:
394  return GridMapCvConverter::addColorLayerFromImage<unsigned char, 3>(cvImage->image, layer, gridMap);
395  case CV_8UC4:
396  return GridMapCvConverter::addColorLayerFromImage<unsigned char, 4>(cvImage->image, layer, gridMap);
397  case CV_16UC3:
398  return GridMapCvConverter::addColorLayerFromImage<unsigned short, 3>(cvImage->image, layer, gridMap);
399  case CV_16UC4:
400  return GridMapCvConverter::addColorLayerFromImage<unsigned short, 4>(cvImage->image, layer, gridMap);
401  default:
402  ROS_ERROR("Expected RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding.");
403  return false;
404  }
405 }
406 
407 bool GridMapRosConverter::toImage(const grid_map::GridMap& gridMap, const std::string& layer,
408  const std::string encoding, sensor_msgs::Image& image)
409 {
410  cv_bridge::CvImage cvImage;
411  if (!toCvImage(gridMap, layer, encoding, cvImage)) return false;
412  cvImage.toImageMsg(image);
413  return true;
414 }
415 
416 bool GridMapRosConverter::toImage(const grid_map::GridMap& gridMap, const std::string& layer,
417  const std::string encoding, const float lowerValue,
418  const float upperValue, sensor_msgs::Image& image)
419 {
420  cv_bridge::CvImage cvImage;
421  if (!toCvImage(gridMap, layer, encoding, lowerValue, upperValue, cvImage)) return false;
422  cvImage.toImageMsg(image);
423  return true;
424 }
425 
426 bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
427  const std::string encoding, cv_bridge::CvImage& cvImage)
428 {
429  const float minValue = gridMap.get(layer).minCoeffOfFinites();
430  const float maxValue = gridMap.get(layer).maxCoeffOfFinites();
431  return toCvImage(gridMap, layer, encoding, minValue, maxValue, cvImage);
432 }
433 
434 bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
435  const std::string encoding, const float lowerValue,
436  const float upperValue, cv_bridge::CvImage& cvImage)
437 {
438  cvImage.header.stamp.fromNSec(gridMap.getTimestamp());
439  cvImage.header.frame_id = gridMap.getFrameId();
440  cvImage.encoding = encoding;
441 
442  const int cvEncoding = cv_bridge::getCvType(encoding);
443  switch (cvEncoding) {
444  case CV_8UC1:
445  return GridMapCvConverter::toImage<unsigned char, 1>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
446  case CV_8UC3:
447  return GridMapCvConverter::toImage<unsigned char, 3>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
448  case CV_8UC4:
449  return GridMapCvConverter::toImage<unsigned char, 4>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
450  case CV_16UC1:
451  return GridMapCvConverter::toImage<unsigned short, 1>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
452  case CV_16UC3:
453  return GridMapCvConverter::toImage<unsigned short, 3>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
454  case CV_16UC4:
455  return GridMapCvConverter::toImage<unsigned short, 4>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
456  default:
457  ROS_ERROR("Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding.");
458  return false;
459  }
460 }
461 
462 bool GridMapRosConverter::saveToBag(const grid_map::GridMap& gridMap, const std::string& pathToBag,
463  const std::string& topic)
464 {
465  grid_map_msgs::GridMap message;
466  toMessage(gridMap, message);
467  ros::Time time;
468  time.fromNSec(gridMap.getTimestamp());
469 
470  if (!time.isValid() || time.isZero()) {
472  time = ros::Time::now();
473  }
474 
475  rosbag::Bag bag;
476  bag.open(pathToBag, rosbag::bagmode::Write);
477  bag.write(topic, time, message);
478  bag.close();
479  return true;
480 }
481 
482 bool GridMapRosConverter::loadFromBag(const std::string& pathToBag, const std::string& topic,
483  grid_map::GridMap& gridMap)
484 {
485  rosbag::Bag bag;
486  bag.open(pathToBag, rosbag::bagmode::Read);
487  rosbag::View view(bag, rosbag::TopicQuery(topic));
488 
489  bool isDataFound = false;
490  for (const auto& messageInstance : view) {
491  grid_map_msgs::GridMap::ConstPtr message = messageInstance.instantiate<grid_map_msgs::GridMap>();
492  if (message != NULL) {
493  fromMessage(*message, gridMap);
494  isDataFound = true;
495  } else {
496  bag.close();
497  ROS_WARN("Unable to load data from ROS bag.");
498  return false;
499  }
500  }
501  if (!isDataFound) ROS_WARN_STREAM("No data under the topic '" << topic << "' was found.");
502  bag.close();
503  return true;
504 }
505 
506 } /* namespace */
const Length & getLength() const
Eigen::Array2i Index
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
Time getTimestamp() const
const Index & getStartIndex() const
void open(std::string const &filename, uint32_t mode=bagmode::Read)
f
bool getPosition(const Index &index, Position &position) const
Time & fromNSec(uint64_t t)
void setBasicLayers(const std::vector< std::string > &basicLayers)
Eigen::Array2i Size
const std::string & getFrameId() const
Eigen::MatrixXf Matrix
std::string encoding
#define ROS_WARN(...)
void close()
bool isValid(const Index &index) const
Eigen::Vector3d Position3
double getResolution() const
const std::vector< std::string > & getLayers() const
Eigen::Vector2d Position
bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_ &m, EigenType_ &e)
static bool isValid()
const Matrix & get(const std::string &layer) const
static void init()
void setStartIndex(const Index &startIndex)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
bool matrixEigenCopyToMultiArrayMessage(const EigenType_ &e, MultiArrayMessageType_ &m)
#define ROS_WARN_STREAM(args)
float & at(const std::string &layer, const Index &index)
int getCvType(const std::string &encoding)
void add(const std::string &layer, const double value=NAN)
#define ROS_INFO_STREAM(args)
int min(int a, int b)
size_t getLinearIndexFromIndex(const Index &index, const Size &bufferSize, const bool rowMajor=false)
static Time now()
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
void setFrameId(const std::string &frameId)
void setTimestamp(const Time timestamp)
#define ROS_ERROR(...)
bool getPosition3(const std::string &layer, const Index &index, Position3 &position) const
Eigen::Array2d Length
void write(std::string const &topic, ros::MessageEvent< T > const &event)
sensor_msgs::ImagePtr toImageMsg() const
const std::vector< std::string > & getBasicLayers() const
std_msgs::Header header
const Size & getSize() const


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