Add sdf param to skip GetGeoRef call when loading DEMs

gazebo/common/Dem.cc

@@ -33,6 +33,11 @@
 #include "gazebo/common/Exception.hh"
 #include "gazebo/common/SphericalCoordinates.hh"
 
+#ifdef __linux__
+#include <unistd.h>
+#include <fcntl.h>
+#endif
+
 using namespace gazebo;
 using namespace common;
 
@@ -58,6 +63,128 @@ Dem::~Dem()
   this->dataPtr = nullptr;
 }
 
+//////////////////////////////////////////////////
+int Dem::LoadWithoutTransform(const std::string &_filename)
+{
+  unsigned int width;
+  unsigned int height;
+  int xSize, ySize;
+  double upLeftX, upLeftY, upRightX, upRightY, lowLeftX, lowLeftY;
+  ignition::math::Angle upLeftLat, upLeftLong, upRightLat, upRightLong;
+  ignition::math::Angle lowLeftLat, lowLeftLong;
+
+  // Sanity check
+  std::string fullName = _filename;
+  if (!boost::filesystem::exists(boost::filesystem::path(fullName)))
+    fullName = common::find_file(_filename);
+
+  if (!boost::filesystem::exists(boost::filesystem::path(fullName)))
+  {
+    gzerr << "Unable to open DEM file[" << _filename
+          << "], check your GAZEBO_RESOURCE_PATH settings." << std::endl;
+    return -1;
+  }
+
+  this->dataPtr->dataSet = reinterpret_cast<GDALDataset *>(GDALOpen(
+    fullName.c_str(), GA_ReadOnly));
+
+  if (this->dataPtr->dataSet == nullptr)
+  {
+    gzerr << "Unable to open DEM file[" << fullName
+          << "]. Format not recognised as a supported dataset." << std::endl;
+    return -1;
+  }
+
+  int nBands = this->dataPtr->dataSet->GetRasterCount();
+  if (nBands != 1)
+  {
+    gzerr << "Unsupported number of bands in file [" << fullName + "]. Found "
+          << nBands << " but only 1 is a valid value." << std::endl;
+    return -1;
+  }
+
+  // Set the pointer to the band
+  this->dataPtr->band = this->dataPtr->dataSet->GetRasterBand(1);
+
+  // Raster width and height
+  xSize = this->dataPtr->dataSet->GetRasterXSize();
+  ySize = this->dataPtr->dataSet->GetRasterYSize();
+
+  // Corner coordinates
+  try
+  {
+    upLeftX = 0.0;
+    upLeftY = 0.0;
+    upRightX = xSize;
+    upRightY = 0.0;
+    lowLeftX = 0.0;
+    lowLeftY = ySize;
+
+    // Set the world width and height
+    this->dataPtr->worldWidth =
+       common::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
+                                              upRightLat, upRightLong);
+    this->dataPtr->worldHeight =
+       common::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
+                                              lowLeftLat, lowLeftLong);
+  }
+  catch(const common::Exception &)
+  {
+    gzwarn << "Failed to automatically compute DEM size. "
+           << "Please use the <size> element to manually set DEM size."
+           << std::endl;
+  }
+
+  // Set the terrain's side (the terrain will be squared after the padding)
+  if (ignition::math::isPowerOfTwo(ySize - 1))
+    height = ySize;
+  else
+    height = ignition::math::roundUpPowerOfTwo(ySize) + 1;
+
+  if (ignition::math::isPowerOfTwo(xSize - 1))
+    width = xSize;
+  else
+    width = ignition::math::roundUpPowerOfTwo(xSize) + 1;
+
+  this->dataPtr->side = std::max(width, height);
+
+  // Preload the DEM's data
+  if (this->LoadData() != 0)
+    return -1;
+
+  // Check for nodata value in dem data. This is used when computing the
+  // min elevation. If nodata value is not defined, we assume it will be one
+  // of the commonly used values such as -9999, -32768, etc.
+  // For simplicity, we will treat values <= -9999 as nodata values and
+  // ignore them when computing the min elevation.
+  int validNoData = 0;
+  const double defaultNoDataValue = -9999;
+  double noDataValue = this->dataPtr->band->GetNoDataValue(&validNoData);
+  if (validNoData <= 0)
+    noDataValue = defaultNoDataValue;
+
+  double min = ignition::math::MAX_D;
+  double max = -ignition::math::MAX_D;
+  for (auto d : this->dataPtr->demData)
+  {
+    if (d < min && d > noDataValue)
+      min = d;
+    if (d > max && d > noDataValue)
+      max = d;
+  }
+  if (ignition::math::equal(min, ignition::math::MAX_D) ||
+      ignition::math::equal(max, -ignition::math::MAX_D))
+  {
+    gzwarn << "Dem is composed of 'nodata' values!" << std::endl;
+  }
+
+  this->dataPtr->minElevation = min;
+  this->dataPtr->maxElevation = max;
+
+  return 0;
+}
+
+
 //////////////////////////////////////////////////
 int Dem::Load(const std::string &_filename)
 {
@@ -230,7 +357,9 @@ void Dem::GetGeoReference(double _x, double _y,
     importString = strdup(this->dataPtr->dataSet->GetProjectionRef());
     sourceCs.importFromWkt(&importString);
     targetCs.SetWellKnownGeogCS("WGS84");
+
     cT = OGRCreateCoordinateTransformation(&sourceCs, &targetCs);
+
     if (nullptr == cT)
     {
       gzthrow("Unable to transform terrain coordinate system to WGS84 for "

gazebo/common/Dem.hh

@@ -54,6 +54,11 @@ namespace gazebo
       /// \return 0 when the operation succeeds to open a file.
       public: int Load(const std::string &_filename="");
 
+      /// \brief Load a DEM file without georeferencing transformation.
+      /// \param[in] _filename the path to the terrain file.
+      /// \return 0 when the operation succeeds to open a file.
+      public: int LoadWithoutTransform(const std::string &_filename="");
+
       /// \brief Get the elevation of a terrain's point in meters.
       /// \param[in] _x X coordinate of the terrain.
       /// \param[in] _y Y coordinate of the terrain.

gazebo/common/HeightmapData.cc

@@ -95,6 +95,47 @@ HeightmapData *HeightmapDataLoader::LoadTerrainFile(
     return LoadDEMAsTerrain(_filename);
   }
 }
+
+//////////////////////////////////////////////////
+HeightmapData *HeightmapDataLoader::LoadTerrainFileWithoutTransform(
+    const std::string &_filename)
+{
+  // Register the GDAL drivers
+  GDALAllRegister();
+
+  GDALDataset *poDataset = reinterpret_cast<GDALDataset *>
+      (GDALOpen(_filename.c_str(), GA_ReadOnly));
+
+  if (!poDataset)
+  {
+    gzerr << "Unrecognized terrain format in file [" << _filename << "]\n";
+    return nullptr;
+  }
+
+  std::string fileFormat = poDataset->GetDriver()->GetDescription();
+  GDALClose(reinterpret_cast<GDALDataset *>(poDataset));
+
+  // Check if the heightmap file is an image
+  if (fileFormat == "JPEG" || fileFormat == "PNG")
+  {
+    // Load the terrain file as an image
+    return LoadImageAsTerrain(_filename);
+  }
+  else
+  {
+    // Load the terrain file as a DEM
+    /* return LoadDEMAsTerrain(_filename); */
+    Dem *dem = new Dem();
+    if (dem->LoadWithoutTransform(_filename) != 0)
+    {
+      gzerr << "Unable to load a DEM file as a terrain [" << _filename << "]\n";
+      return nullptr;
+    }
+    return static_cast<HeightmapData *>(dem);
+  }
+}
+
+
 #else
 HeightmapData *HeightmapDataLoader::LoadTerrainFile(
     const std::string &_filename)

gazebo/common/HeightmapData.hh

@@ -82,6 +82,16 @@ namespace gazebo
       public: static HeightmapData *LoadTerrainFile(
           const std::string &_filename);
 
+      /// \brief Load a terrain file specified by _filename. The terrain file
+      /// format might be an image or a DEM file. libgdal is required to enable
+      /// DEM support. For a list of all raster formats supported you can type
+      /// the command "gdalinfo --formats". Skips the georeferencing
+      /// transformation.
+      /// \param[in] _filename The path to the terrain file.
+      /// \return 0 when the operation succeeds to load a file or -1 when fails.
+      public: static HeightmapData *LoadTerrainFileWithoutTransform(
+          const std::string &_filename);
+
       /// \brief Load a DEM specified by _filename as a terrain file.
       /// \param[in] _filename The path to the terrain file.
       /// \return 0 when the operation succeeds to load a file or -1 when fails.

gazebo/physics/HeightmapShape.cc

@@ -81,7 +81,20 @@ void HeightmapShape::OnRequest(ConstRequestPtr &_msg)
 //////////////////////////////////////////////////
 int HeightmapShape::LoadTerrainFile(const std::string &_filename)
 {
-  this->heightmapData = common::HeightmapDataLoader::LoadTerrainFile(_filename);
+  bool skipGeoRef = false;
+  if (this->sdf->HasElement("skipGeoRef"))
+  {
+    skipGeoRef = this->sdf->Get<bool>("skipGeoRef");
+  }
+
+  if (skipGeoRef)
+  {
+    this->heightmapData = common::HeightmapDataLoader::LoadTerrainFileWithoutTransform(_filename);
+  }
+  else
+  {
+    this->heightmapData = common::HeightmapDataLoader::LoadTerrainFile(_filename);
+  }
   if (!this->heightmapData)
   {
     gzerr << "Unable to load heightmap data" << std::endl;
@@ -118,7 +131,10 @@ int HeightmapShape::LoadTerrainFile(const std::string &_filename)
 
       try
       {
-        this->dem.GetGeoReferenceOrigin(latitude, longitude);
+        if (!skipGeoRef)
+        {
+          this->dem.GetGeoReferenceOrigin(latitude, longitude);
+        }
       }
       catch(const common::Exception &)
       {