Skip to content

Commit

Permalink
Support lunar DEMs (#3257)
Browse files Browse the repository at this point in the history
* SphericalCoordinates: support Lunar calculations (#3250)
  Add support for MOON_SCS in spherical coords

Signed-off-by: Aditya <[email protected]>

* Use DistanceBetweenPoints when loading DEMs (#3252)

Signed-off-by: Aditya <[email protected]>
Signed-off-by: Steve Peters <[email protected]>
Co-authored-by: Steve Peters <[email protected]>

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

Signed-off-by: Aditya <[email protected]>
Co-authored-by: Steve Peters <[email protected]>
  • Loading branch information
adityapande-1995 and scpeters authored Nov 4, 2022
1 parent 94adc32 commit d86d435
Show file tree
Hide file tree
Showing 15 changed files with 495 additions and 17 deletions.
41 changes: 35 additions & 6 deletions gazebo/common/Dem.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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 &)
{
Expand Down Expand Up @@ -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)
{
Expand Down
7 changes: 7 additions & 0 deletions gazebo/common/Dem.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
5 changes: 5 additions & 0 deletions gazebo/common/DemPrivate.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand Down Expand Up @@ -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>();
};
/// \}
}
Expand Down
28 changes: 28 additions & 0 deletions gazebo/common/Dem_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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

/////////////////////////////////////////////////
Expand Down
53 changes: 52 additions & 1 deletion gazebo/common/HeightmapData.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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"


Expand All @@ -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";
Expand All @@ -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();
Expand All @@ -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
35 changes: 32 additions & 3 deletions gazebo/common/HeightmapData.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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);
};
Expand Down
33 changes: 33 additions & 0 deletions gazebo/common/HeightmapData_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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

/////////////////////////////////////////////////
Expand Down
Loading

0 comments on commit d86d435

Please sign in to comment.