void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, const std::vector<std::string>& layers,
grid_map_msgs::GridMap& message)
{
message.info.header.stamp.fromNSec(gridMap.getTimestamp());
message.info.header.frame_id = gridMap.getFrameId();
message.info.resolution = gridMap.getResolution();
message.info.length_x = gridMap.getLength().x();
message.info.length_y = gridMap.getLength().y();
message.info.pose.position.x = gridMap.getPosition().x();
message.info.pose.position.y = gridMap.getPosition().y();
message.info.pose.position.z = 0.0;
message.info.pose.orientation.x = 0.0;
message.info.pose.orientation.y = 0.0;
message.info.pose.orientation.z = 0.0;
message.info.pose.orientation.w = 1.0;
message.layers = layers;
message.basic_layers = gridMap.getBasicLayers();
message.data.clear();
for (const auto& layer : layers) {
std_msgs::Float32MultiArray dataArray;
matrixEigenCopyToMultiArrayMessage(gridMap.get(layer), dataArray);
message.data.push_back(dataArray);
}
message.outer_start_index = gridMap.getStartIndex()(0);
message.inner_start_index = gridMap.getStartIndex()(1);
}
bool LineIterator::initialize(const grid_map::GridMap& gridMap, const Index& start, const Index& end)
{
start_ = start;
end_ = end;
mapLength_ = gridMap.getLength();
mapPosition_ = gridMap.getPosition();
resolution_ = gridMap.getResolution();
bufferSize_ = gridMap.getSize();
bufferStartIndex_ = gridMap.getStartIndex();
initializeIterationParameters();
return true;
}
PolygonIterator::PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon)
: polygon_(polygon)
{
mapLength_ = gridMap.getLength();
mapPosition_ = gridMap.getPosition();
resolution_ = gridMap.getResolution();
bufferSize_ = gridMap.getSize();
bufferStartIndex_ = gridMap.getStartIndex();
Index submapStartIndex;
Size submapBufferSize;
findSubmapParameters(polygon, submapStartIndex, submapBufferSize);
internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
if(!isInside()) ++(*this);
}
bool GridMapRosConverter::fromOccupancyGrid(const nav_msgs::OccupancyGrid& occupancyGrid,
const std::string& layer, grid_map::GridMap& gridMap)
{
const Size size(occupancyGrid.info.width, occupancyGrid.info.height);
const double resolution = occupancyGrid.info.resolution;
const Length length = resolution * size.cast<double>();
const string& frameId = occupancyGrid.header.frame_id;
Position position(occupancyGrid.info.origin.position.x, occupancyGrid.info.origin.position.y);
// Different conventions of center of map.
position += 0.5 * length.matrix();
const auto& orientation = occupancyGrid.info.origin.orientation;
if (orientation.w != 1.0 && !(orientation.x == 0 && orientation.y == 0
&& orientation.z == 0 && orientation.w == 0)) {
ROS_WARN_STREAM("Conversion of occupancy grid: Grid maps do not support orientation.");
ROS_INFO_STREAM("Orientation of occupancy grid: " << endl << occupancyGrid.info.origin.orientation);
return false;
}
if (size.prod() != occupancyGrid.data.size()) {
ROS_WARN_STREAM("Conversion of occupancy grid: Size of data does not correspond to width * height.");
return false;
}
if ((gridMap.getSize() != size).any() || gridMap.getResolution() != resolution
|| (gridMap.getLength() != length).any() || gridMap.getPosition() != position
|| gridMap.getFrameId() != frameId || !gridMap.getStartIndex().isZero()) {
gridMap.setTimestamp(occupancyGrid.header.stamp.toNSec());
gridMap.setFrameId(frameId);
gridMap.setGeometry(length, resolution, position);
}
// Reverse iteration is required because of different conventions
// between occupancy grid and grid map.
grid_map::Matrix data(size(0), size(1));
for (std::vector<int8_t>::const_reverse_iterator iterator = occupancyGrid.data.rbegin();
iterator != occupancyGrid.data.rend(); ++iterator) {
size_t i = std::distance(occupancyGrid.data.rbegin(), iterator);
data(i) = *iterator != -1 ? *iterator : NAN;
}
gridMap.add(layer, data);
return true;
}
