15 #include <geometry_msgs/Point.h> 16 #include <geometry_msgs/Quaternion.h> 26 using namespace Eigen;
30 GridMapRosConverter::GridMapRosConverter()
34 GridMapRosConverter::~GridMapRosConverter()
38 bool GridMapRosConverter::fromMessage(
const grid_map_msgs::GridMap& message,
grid_map::GridMap& gridMap,
const std::vector<std::string>& layers,
bool copyBasicLayers,
bool copyAllNonBasicLayers)
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));
45 if (message.layers.size() != message.data.size()) {
46 ROS_ERROR(
"Different number of layers and data in grid map message.");
51 for (
unsigned int i = 0u; i < message.layers.size(); ++i) {
54 if (!copyAllNonBasicLayers && std::find(layers.begin(), layers.end(), message.layers[i]) == layers.end()) {
65 gridMap.
add(message.layers[i], data);
69 if (copyBasicLayers) {
77 bool GridMapRosConverter::fromMessage(
const grid_map_msgs::GridMap& message,
grid_map::GridMap& gridMap)
79 return fromMessage(message, gridMap, std::vector<std::string>(),
true,
true);
82 void GridMapRosConverter::toMessage(
const grid_map::GridMap& gridMap, grid_map_msgs::GridMap& message)
84 toMessage(gridMap, gridMap.
getLayers(), message);
87 void GridMapRosConverter::toMessage(
const grid_map::GridMap& gridMap,
const std::vector<std::string>& layers,
88 grid_map_msgs::GridMap& message)
90 message.info.header.stamp.fromNSec(gridMap.
getTimestamp());
91 message.info.header.frame_id = gridMap.
getFrameId();
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;
103 message.layers = layers;
106 message.data.clear();
107 for (
const auto& layer : layers) {
108 std_msgs::Float32MultiArray dataArray;
110 message.data.push_back(dataArray);
118 const std::string& pointLayer,
119 sensor_msgs::PointCloud2& pointCloud)
121 toPointCloud(gridMap, gridMap.
getLayers(), pointLayer, pointCloud);
125 const std::vector<std::string>& layers,
126 const std::string& pointLayer,
127 sensor_msgs::PointCloud2& pointCloud)
130 pointCloud.header.frame_id = gridMap.
getFrameId();
131 pointCloud.header.stamp.fromNSec(gridMap.
getTimestamp());
132 pointCloud.is_dense =
false;
135 std::vector <std::string> fieldNames;
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");
145 fieldNames.push_back(layer);
149 pointCloud.fields.clear();
150 pointCloud.fields.reserve(fieldNames.size());
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;
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);
172 std::unordered_map<std::string, sensor_msgs::PointCloud2Iterator<float>> fieldIterators;
173 for (
auto& name : fieldNames) {
174 fieldIterators.insert(
182 size_t realNumberOfPoints = 0;
183 for (
size_t i = 0; i < maxNumberOfPoints; ++i) {
184 if (hasBasicLayers) {
185 if (!gridMap.
isValid(*mapIterator)) {
192 if (!gridMap.
getPosition3(pointLayer, *mapIterator, position)) {
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);
207 *iterator.second = gridMap.
at(iterator.first, *mapIterator);
212 for (
auto& iterator : fieldIterators) {
215 ++realNumberOfPoints;
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);
225 bool GridMapRosConverter::fromOccupancyGrid(
const nav_msgs::OccupancyGrid& occupancyGrid,
228 const Size size(occupancyGrid.info.width, occupancyGrid.info.height);
229 const double resolution = occupancyGrid.info.resolution;
231 const string& frameId = occupancyGrid.header.frame_id;
232 Position position(occupancyGrid.info.origin.position.x, occupancyGrid.info.origin.position.y);
234 position += 0.5 * length.matrix();
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);
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.");
253 gridMap.
setTimestamp(occupancyGrid.header.stamp.toNSec());
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;
267 gridMap.
add(layer, data);
272 const std::string& layer,
float dataMin,
float dataMax,
273 nav_msgs::OccupancyGrid& occupancyGrid)
275 occupancyGrid.header.frame_id = gridMap.
getFrameId();
276 occupancyGrid.header.stamp.fromNSec(gridMap.
getTimestamp());
277 occupancyGrid.info.map_load_time = occupancyGrid.header.stamp;
279 occupancyGrid.info.width = gridMap.
getSize()(0);
280 occupancyGrid.info.height = gridMap.
getSize()(1);
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);
293 const float cellMin = 0;
294 const float cellMax = 100;
295 const float cellRange = cellMax - cellMin;
298 float value = (gridMap.
at(layer, *iterator) - dataMin) / (dataMax - dataMin);
302 value = cellMin +
min(max(0.0
f, value), 1.0
f) * cellRange;
305 occupancyGrid.data[nCells - index - 1] = value;
309 void GridMapRosConverter::toGridCells(
const grid_map::GridMap& gridMap,
const std::string& layer,
310 float lowerThreshold,
float upperThreshold,
311 nav_msgs::GridCells& gridCells)
313 gridCells.header.frame_id = gridMap.
getFrameId();
314 gridCells.header.stamp.fromNSec(gridMap.
getTimestamp());
319 if (!gridMap.
isValid(*iterator, layer))
continue;
320 if (gridMap.
at(layer, *iterator) >= lowerThreshold
321 && gridMap.
at(layer, *iterator) <= upperThreshold) {
324 geometry_msgs::Point point;
325 point.x = position.x();
326 point.y = position.y();
327 gridCells.cells.push_back(point);
332 bool GridMapRosConverter::initializeFromImage(
const sensor_msgs::Image& image,
336 const double lengthX = resolution * image.height;
337 const double lengthY = resolution * image.width;
345 bool GridMapRosConverter::addLayerFromImage(
const sensor_msgs::Image& image,
347 const float lowerValue,
const float upperValue,
348 const double alphaThreshold)
355 ROS_ERROR(
"cv_bridge exception: %s", e.what());
360 switch (cvEncoding) {
362 return GridMapCvConverter::addLayerFromImage<unsigned char, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
364 return GridMapCvConverter::addLayerFromImage<unsigned char, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
366 return GridMapCvConverter::addLayerFromImage<unsigned char, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
368 return GridMapCvConverter::addLayerFromImage<unsigned short, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
370 return GridMapCvConverter::addLayerFromImage<unsigned short, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
372 return GridMapCvConverter::addLayerFromImage<unsigned short, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
374 ROS_ERROR(
"Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding.");
379 bool GridMapRosConverter::addColorLayerFromImage(
const sensor_msgs::Image& image,
380 const std::string& layer,
387 ROS_ERROR(
"cv_bridge exception: %s", e.what());
392 switch (cvEncoding) {
394 return GridMapCvConverter::addColorLayerFromImage<unsigned char, 3>(cvImage->image, layer, gridMap);
396 return GridMapCvConverter::addColorLayerFromImage<unsigned char, 4>(cvImage->image, layer, gridMap);
398 return GridMapCvConverter::addColorLayerFromImage<unsigned short, 3>(cvImage->image, layer, gridMap);
400 return GridMapCvConverter::addColorLayerFromImage<unsigned short, 4>(cvImage->image, layer, gridMap);
402 ROS_ERROR(
"Expected RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding.");
408 const std::string encoding, sensor_msgs::Image& image)
411 if (!toCvImage(gridMap, layer, encoding, cvImage))
return false;
417 const std::string encoding,
const float lowerValue,
418 const float upperValue, sensor_msgs::Image& image)
421 if (!toCvImage(gridMap, layer, encoding, lowerValue, upperValue, cvImage))
return false;
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);
435 const std::string encoding,
const float lowerValue,
443 switch (cvEncoding) {
445 return GridMapCvConverter::toImage<unsigned char, 1>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.
image);
447 return GridMapCvConverter::toImage<unsigned char, 3>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.
image);
449 return GridMapCvConverter::toImage<unsigned char, 4>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.
image);
451 return GridMapCvConverter::toImage<unsigned short, 1>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.
image);
453 return GridMapCvConverter::toImage<unsigned short, 3>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.
image);
455 return GridMapCvConverter::toImage<unsigned short, 4>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.
image);
457 ROS_ERROR(
"Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding.");
462 bool GridMapRosConverter::saveToBag(
const grid_map::GridMap& gridMap,
const std::string& pathToBag,
463 const std::string& topic)
465 grid_map_msgs::GridMap message;
466 toMessage(gridMap, message);
477 bag.
write(topic, time, message);
482 bool GridMapRosConverter::loadFromBag(
const std::string& pathToBag,
const std::string& topic,
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);
497 ROS_WARN(
"Unable to load data from ROS bag.");
501 if (!isDataFound)
ROS_WARN_STREAM(
"No data under the topic '" << topic <<
"' was found.");
const Length & getLength() const
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)
bool getPosition(const Index &index, Position &position) const
Time & fromNSec(uint64_t t)
void setBasicLayers(const std::vector< std::string > &basicLayers)
const std::string & getFrameId() const
bool isValid(const Index &index) const
Eigen::Vector3d Position3
double getResolution() const
const std::vector< std::string > & getLayers() const
bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_ &m, EigenType_ &e)
const Matrix & get(const std::string &layer) const
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)
size_t getLinearIndexFromIndex(const Index &index, const Size &bufferSize, const bool rowMajor=false)
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
void setFrameId(const std::string &frameId)
void setTimestamp(const Time timestamp)
bool getPosition3(const std::string &layer, const Index &index, Position3 &position) const
void write(std::string const &topic, ros::MessageEvent< T > const &event)
sensor_msgs::ImagePtr toImageMsg() const
const std::vector< std::string > & getBasicLayers() const
const Size & getSize() const