Method fromMessage

grid_map_ros/src/GridMapRosConverter.cpp:38–75

}

bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap, const std::vector<std::string>& layers, bool copyBasicLayers, bool copyAllNonBasicLayers)
{
  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;
  }

  // Copy non-basic layers.
  for (unsigned int i = 0u; i < message.layers.size(); ++i) {

    // check if layer should be copied.
    if (!copyAllNonBasicLayers && std::find(layers.begin(), layers.end(), message.layers[i]) == layers.end()) {
      continue;
    }

    // TODO Could we use the data mapping (instead of copying) method here?
    Matrix data;
    if(!multiArrayMessageCopyToMatrixEigen(message.data[i], data)) {
      return false;
    }

    // TODO Check if size is good.   size_ << getRows(message.data[0]), getCols(message.data[0]);
    gridMap.add(message.layers[i], data);
  }

  // Copy basic layers.
  if (copyBasicLayers) {
    gridMap.setBasicLayers(message.basic_layers);
  }

  gridMap.setStartIndex(Index(message.outer_start_index, message.inner_start_index));
  return true;
}

bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap)
{