Method update

uav_simulator/Utils/rviz_plugins/src/probmap_display.cpp:219–428

}

void ProbMapDisplay::update(float wall_dt, float ros_dt) {
  {
    boost::mutex::scoped_lock lock(mutex_);

    current_map_ = updated_map_;
  }

  if (!current_map_ || !new_map_) {
    return;
  }

  if (current_map_->data.empty()) {
    return;
  }

  new_map_ = false;

  if (!validateFloats(*current_map_)) {
    setStatus(StatusProperty::Error, "Map", "Message contained invalid floating point values (nans or "
                                            "infs)");
    return;
  }

  if (current_map_->info.width * current_map_->info.height == 0) {
    std::stringstream ss;
    ss << "Map is zero-sized (" << current_map_->info.width << "x" << current_map_->info.height << ")";
    setStatus(StatusProperty::Error, "Map", QString::fromStdString(ss.str()));
    return;
  }

  clear();

  setStatus(StatusProperty::Ok, "Message", "Map received");

  ROS_DEBUG("Received a %d X %d map @ %.3f m/pix\n", current_map_->info.width, current_map_->info.height,
            current_map_->info.resolution);

  float resolution = current_map_->info.resolution;

  int width = current_map_->info.width;
  int height = current_map_->info.height;

  Ogre::Vector3 position(current_map_->info.origin.position.x, current_map_->info.origin.position.y,
                         current_map_->info.origin.position.z);
  Ogre::Quaternion orientation(
      current_map_->info.origin.orientation.w, current_map_->info.origin.orientation.x,
      current_map_->info.origin.orientation.y, current_map_->info.origin.orientation.z);
  frame_ = current_map_->header.frame_id;
  if (frame_.empty()) {
    frame_ = "/map";
  }

  // Expand it to be RGB data
  unsigned int pixels_size = width * height;
  unsigned char* pixels = new unsigned char[pixels_size];
  memset(pixels, 255, pixels_size);

  bool map_status_set = false;