/*! * Initializes the geometry of a grid map from an image. This changes * the geometry of the map and deletes all contents of the layers! * @param[in] image the image. * @param[in] resolution the desired resolution of the grid map [m/cell]. * @param[out] gridMap the grid map to be initialized. * @param[in](optional) position the position of the grid map. * @return true if successful, false otherwise. */ static bool initializeFromImage(const cv::Mat& image, const double resolution, grid_map::GridMap& gridMap, const grid_map::Position& position) { const double lengthX = resolution * image.rows; const double lengthY = resolution * image.cols; Length length(lengthX, lengthY); gridMap.setGeometry(length, resolution, position); return true; }
bool GridMapRosConverter::initializeFromImage(const sensor_msgs::Image& image, const double resolution, grid_map::GridMap& gridMap, const grid_map::Position& position) { const double lengthX = resolution * image.height; const double lengthY = resolution * image.width; Length length(lengthX, lengthY); gridMap.setGeometry(length, resolution, position); gridMap.setFrameId(image.header.frame_id); gridMap.setTimestamp(image.header.stamp.toNSec()); return true; }
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; }
bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap) { gridMap.setTimestamp(message.info.header.stamp.toNSec()); gridMap.setFrameId(message.info.header.frame_id); gridMap.setGeometry(Length(message.info.length_x, message.info.length_y), message.info.resolution, Position(message.info.pose.position.x, message.info.pose.position.y)); if (message.layers.size() != message.data.size()) { ROS_ERROR("Different number of layers and data in grid map message."); return false; } for (unsigned int i = 0; i < message.layers.size(); i++) { Matrix data; multiArrayMessageCopyToMatrixEigen(message.data[i], data); // TODO Could we use the data mapping (instead of copying) method here? // TODO Check if size is good. size_ << getRows(message.data[0]), getCols(message.data[0]); gridMap.add(message.layers[i], data); } gridMap.setBasicLayers(message.basic_layers); gridMap.setStartIndex(Index(message.outer_start_index, message.inner_start_index)); return true; }