Support lunar DEMs (#3257)

* SphericalCoordinates: support Lunar calculations (#3250)
  Add support for MOON_SCS in spherical coords

Signed-off-by: Aditya <aditya050995@gmail.com>

* Use DistanceBetweenPoints when loading DEMs (#3252)

Signed-off-by: Aditya <aditya050995@gmail.com>
Signed-off-by: Steve Peters <scpeters@openrobotics.org>
Co-authored-by: Steve Peters <scpeters@openrobotics.org>

* Make Heightmap loader use spherical coordinates. (#3258)
  Make Physics.cc and Gazebo GUI use spherical coordinates
  when loading terrains.

Signed-off-by: Aditya <aditya050995@gmail.com>
Co-authored-by: Steve Peters <scpeters@openrobotics.org>

gazebo/common/Dem.cc

@@ -58,6 +58,14 @@ Dem::~Dem()
   this->dataPtr = nullptr;
 }
 
+//////////////////////////////////////////////////
+void Dem::SetSphericalCoordinates(
+    common::SphericalCoordinatesPtr _worldSphericalCoordinates)
+{
+  this->dataPtr->sphericalCoordinates =_worldSphericalCoordinates;
+}
+
+
 //////////////////////////////////////////////////
 int Dem::Load(const std::string &_filename)
 {
@@ -122,11 +130,11 @@ int Dem::Load(const std::string &_filename)
 
     // Set the world width and height
     this->dataPtr->worldWidth =
-       common::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
-                                              upRightLat, upRightLong);
+       this->dataPtr->sphericalCoordinates->DistanceBetweenPoints(
+           upLeftLat, upLeftLong, upRightLat, upRightLong);
     this->dataPtr->worldHeight =
-       common::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
-                                              lowLeftLat, lowLeftLong);
+       this->dataPtr->sphericalCoordinates->DistanceBetweenPoints(
+           upLeftLat, upLeftLong, lowLeftLat, lowLeftLong);
   }
   catch(const common::Exception &)
   {
@@ -221,15 +229,36 @@ void Dem::GetGeoReference(double _x, double _y,
     OGRCoordinateTransformation *cT;
     double xGeoDeg, yGeoDeg;
 
-    // Transform the terrain's coordinate system to WGS84
+    // Transform the terrain's coordinate system to the appropriate
+    // coordinate system.
     #if GDAL_VERSION_NUM >= 2030000
     const char *importString;
     #else
     char *importString;
     #endif
     importString = strdup(this->dataPtr->dataSet->GetProjectionRef());
     sourceCs.importFromWkt(&importString);
-    targetCs.SetWellKnownGeogCS("WGS84");
+    if (this->dataPtr->sphericalCoordinates->GetSurfaceType() ==
+        common::SphericalCoordinates::MOON_SCS)
+    {
+      targetCs = OGRSpatialReference();
+
+      double axisEquatorial =
+        this->dataPtr->sphericalCoordinates->SurfaceAxisEquatorial();
+      double axisPolar =
+        this->dataPtr->sphericalCoordinates->SurfaceAxisPolar();
+
+      std::string surfaceLatLongProjStr =
+        "+proj=latlong +a=" + std::to_string(axisEquatorial) +
+        " +b=" + std::to_string(axisPolar);
+
+      targetCs.importFromProj4(surfaceLatLongProjStr.c_str());
+    }
+    else
+    {
+      targetCs.SetWellKnownGeogCS("WGS84");
+    }
+
     cT = OGRCreateCoordinateTransformation(&sourceCs, &targetCs);
     if (nullptr == cT)
     {

gazebo/common/Dem.hh

@@ -49,6 +49,13 @@ namespace gazebo
       /// \brief Destructor.
       public: virtual ~Dem();
 
+      /// \brief Sets the spherical coordinates reference object.
+      /// \param[in] _worldSphericalCoordinates The spherical coordiantes
+      /// object contained in the world. This is used to compute accurate
+      /// sizes of the DEM object.
+      public: void SetSphericalCoordinates(
+                  common::SphericalCoordinatesPtr _worldSphericalCoordinates);
+
       /// \brief Load a DEM file.
       /// \param[in] _filename the path to the terrain file.
       /// \return 0 when the operation succeeds to open a file.

gazebo/common/DemPrivate.hh

@@ -18,6 +18,7 @@
 #ifndef _GAZEBO_DEM_PRIVATE_HH_
 #define _GAZEBO_DEM_PRIVATE_HH_
 
+#include "gazebo/common/SphericalCoordinates.hh"
 #include <gazebo/gazebo_config.h>
 #include <gazebo/util/system.hh>
 
@@ -59,6 +60,10 @@ namespace gazebo
 
       /// \brief DEM data converted to be OGRE-compatible.
       public: std::vector<float> demData;
+
+      /// \brief Holds the spherical coordinates object from the world.
+      public: common::SphericalCoordinatesPtr sphericalCoordinates =
+              boost::make_shared<common::SphericalCoordinates>();
     };
     /// \}
   }

gazebo/common/Dem_TEST.cc

@@ -21,6 +21,7 @@
 #include <ignition/math/Helpers.hh>
 #include <ignition/math/Vector3.hh>
 
+#include "gazebo/common/SphericalCoordinates.hh"
 #include "gazebo/common/Dem.hh"
 #include "test_config.h"
 #include "test/util.hh"
@@ -223,6 +224,33 @@ TEST_F(DemTest, UnfinishedDem)
   EXPECT_FLOAT_EQ(682, demNoData.GetMinElevation());
   EXPECT_FLOAT_EQ(2932, demNoData.GetMaxElevation());
 }
+
+/////////////////////////////////////////////////
+TEST_F(DemTest, LunarDemLoad)
+{
+  common::Dem dem;
+  boost::filesystem::path path = TEST_PATH;
+  path /= "data/dem_moon.tif";
+
+  // Sizes will be computed incorrectly
+  // as the celestial bodies in DEM file and
+  // default spherical coordinates do not match.
+  EXPECT_EQ(dem.Load(path.string()), 0);
+  EXPECT_NEAR(293.51, dem.GetWorldWidth(), 0.1);
+  EXPECT_NEAR(293.51, dem.GetWorldHeight(), 0.1);
+
+  // Setting the spherical coordinates solves the
+  // problem.
+  common::SphericalCoordinatesPtr moonSC =
+    boost::make_shared<common::SphericalCoordinates>(
+        common::SphericalCoordinates::MOON_SCS);
+
+  dem.SetSphericalCoordinates(moonSC);
+  EXPECT_EQ(dem.Load(path.string()), 0);
+
+  EXPECT_FLOAT_EQ(80.0417, dem.GetWorldWidth());
+  EXPECT_FLOAT_EQ(80.0417, dem.GetWorldHeight());
+}
 #endif
 
 /////////////////////////////////////////////////

gazebo/common/HeightmapData.cc

@@ -28,6 +28,7 @@
 #include "gazebo/common/Console.hh"
 #include "gazebo/common/ImageHeightmap.hh"
 #include "gazebo/common/HeightmapData.hh"
+#include "gazebo/common/SphericalCoordinates.hh"
 #include "gazebo/common/Dem.hh"
 
 
@@ -54,8 +55,20 @@ HeightmapData *HeightmapDataLoader::LoadImageAsTerrain(
 //////////////////////////////////////////////////
 HeightmapData *HeightmapDataLoader::LoadDEMAsTerrain(
     const std::string &_filename)
+{
+  return LoadDEMAsTerrain(_filename,
+                          boost::make_shared<common::SphericalCoordinates>());
+}
+
+//////////////////////////////////////////////////
+/// Load terrain files taking into account the spherical
+/// coordinates surface type.
+HeightmapData *HeightmapDataLoader::LoadDEMAsTerrain(
+    const std::string &_filename,
+    common::SphericalCoordinatesPtr _sphericalCoordinates)
 {
   Dem *dem = new Dem();
+  dem->SetSphericalCoordinates(_sphericalCoordinates);
   if (dem->Load(_filename) != 0)
   {
     gzerr << "Unable to load a DEM file as a terrain [" << _filename << "]\n";
@@ -67,6 +80,17 @@ HeightmapData *HeightmapDataLoader::LoadDEMAsTerrain(
 //////////////////////////////////////////////////
 HeightmapData *HeightmapDataLoader::LoadTerrainFile(
     const std::string &_filename)
+{
+  return LoadTerrainFile(_filename,
+                         boost::make_shared<common::SphericalCoordinates>());
+}
+
+//////////////////////////////////////////////////
+/// Load terrain files taking into account the spherical
+/// coordinates surface type.
+HeightmapData *HeightmapDataLoader::LoadTerrainFile(
+    const std::string &_filename,
+    common::SphericalCoordinatesPtr _sphericalCoordinates)
 {
   // Register the GDAL drivers
   GDALAllRegister();
@@ -92,14 +116,41 @@ HeightmapData *HeightmapDataLoader::LoadTerrainFile(
   else
   {
     // Load the terrain file as a DEM
-    return LoadDEMAsTerrain(_filename);
+    return LoadDEMAsTerrain(_filename, _sphericalCoordinates);
   }
 }
 #else
+//////////////////////////////////////////////////
+HeightmapData *HeightmapDataLoader::LoadDEMAsTerrain(
+    const std::string &_filename)
+{
+  gzerr << "GDAL not available, LoadDEMAsTerrain will not work"
+    << std::endl;
+}
+
+//////////////////////////////////////////////////
+HeightmapData *HeightmapDataLoader::LoadDEMAsTerrain(
+    const std::string &_filename,
+    common::SphericalCoordinatesPtr _sphericalCoordinates)
+{
+  gzerr << "GDAL not available, LoadDEMAsTerrain will not work"
+    << std::endl;
+}
+
+//////////////////////////////////////////////////
 HeightmapData *HeightmapDataLoader::LoadTerrainFile(
     const std::string &_filename)
 {
   // Load the terrain file as an image
   return LoadImageAsTerrain(_filename);
 }
+
+//////////////////////////////////////////////////
+HeightmapData *HeightmapDataLoader::LoadTerrainFile(
+    const std::string &_filename,
+    common::SphericalCoordinatesPtr /*_sphericalCoordinates*/)
+{
+  // Load the terrain file as an image
+  return LoadImageAsTerrain(_filename);
+}
 #endif

gazebo/common/HeightmapData.hh

@@ -25,6 +25,7 @@
 #include "gazebo/gazebo_config.h"
 
 #include "gazebo/common/CommonTypes.hh"
+#include "gazebo/common/SphericalCoordinates.hh"
 #include "gazebo/util/system.hh"
 
 namespace gazebo
@@ -78,19 +79,47 @@ namespace gazebo
       /// DEM support. For a list of all raster formats supported you can type
       /// the command "gdalinfo --formats".
       /// \param[in] _filename The path to the terrain file.
-      /// \return 0 when the operation succeeds to load a file or -1 when fails.
+      /// \return pointer to memory containing the loaded heightmap data
+      /// when the operation succeeds to load a file or a nullptr when it fails.
       public: static HeightmapData *LoadTerrainFile(
           const std::string &_filename);
 
+      /// \brief Load a terrain file specified by _filename, with
+      /// _sphericalCoordinates as the surface type. 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".
+      /// \param[in] _filename The path to the terrain file.
+      /// \param[in] _sphericalCoordinates SphericalCoordinatesPtr from the
+      /// world.
+      /// \return pointer to memory containing the loaded heightmap data
+      /// when the operation succeeds to load a file or a nullptr when it fails.
+      public: static HeightmapData *LoadTerrainFile(
+          const std::string &_filename,
+          common::SphericalCoordinatesPtr _sphericalCoordinates);
+
       /// \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.
+      /// \return pointer to memory containing the loaded heightmap data
+      /// when the operation succeeds to load a file or a nullptr when it fails.
       private: static HeightmapData *LoadDEMAsTerrain(
           const std::string &_filename);
 
+      /// \brief Load a DEM specified by _filename as a terrain file using
+      /// _sphericalCoordinates as the surface type.
+      /// \param[in] _filename The path to the terrain file.
+      /// \param[in] _sphericalCoordinates SphericalCoordinatesPtr from the
+      /// world.
+      /// \return pointer to memory containing the loaded heightmap data
+      /// when the operation succeeds to load a file or a nullptr when it fails.
+      private: static HeightmapData *LoadDEMAsTerrain(
+          const std::string &_filename,
+          common::SphericalCoordinatesPtr _sphericalCoordinates);
+
       /// \brief Load an image 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.
+      /// \return pointer to memory containing the loaded heightmap data
+      /// when the operation succeeds to load a file or a nullptr when it fails.
       private: static HeightmapData *LoadImageAsTerrain(
           const std::string &_filename);
     };

gazebo/common/HeightmapData_TEST.cc

@@ -87,6 +87,39 @@ TEST_F(HeightmapDataLoaderTest, DemHeightmap)
   EXPECT_FLOAT_EQ(142.2274, dem->GetElevation(0, height - 1));
   EXPECT_FLOAT_EQ(209.14784, dem->GetElevation(width - 1, height - 1));
 }
+
+/////////////////////////////////////////////////
+TEST_F(HeightmapDataLoaderTest, DemHeightmapMoon)
+{
+  common::HeightmapData *heightmapData = NULL;
+
+  boost::filesystem::path path = TEST_PATH;
+  path /= "data/dem_moon.tif";
+
+  heightmapData = common::HeightmapDataLoader::LoadTerrainFile(path.string());
+  EXPECT_TRUE(heightmapData != nullptr);
+
+  common::Dem *dem = dynamic_cast<common::Dem *>(heightmapData);
+  EXPECT_TRUE(dem != nullptr);
+
+  // Width and height will not be accurately calculated,
+  // and an error will be displayed.
+  EXPECT_GT(dem->GetWorldHeight(), 290);
+  EXPECT_GT(dem->GetWorldWidth(), 290);
+
+  // Using MOON_SCS surface type
+  heightmapData =
+    common::HeightmapDataLoader::LoadTerrainFile(path.string(),
+        boost::make_shared<common::SphericalCoordinates>(
+          common::SphericalCoordinates::MOON_SCS));
+  EXPECT_TRUE(heightmapData != nullptr);
+
+  dem = dynamic_cast<common::Dem *>(heightmapData);
+  EXPECT_TRUE(dem != nullptr);
+  // The sizes will now be computed correctly.
+  EXPECT_FLOAT_EQ(80.0417, dem->GetWorldHeight());
+  EXPECT_FLOAT_EQ(80.0417, dem->GetWorldWidth());
+}
 #endif
 
 /////////////////////////////////////////////////