From d86d435c7b153a0c182ff61a62916970437e59bd Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Fri, 4 Nov 2022 10:21:43 -0700 Subject: [PATCH] Support lunar DEMs (#3257) * SphericalCoordinates: support Lunar calculations (#3250) Add support for MOON_SCS in spherical coords Signed-off-by: Aditya * Use DistanceBetweenPoints when loading DEMs (#3252) Signed-off-by: Aditya Signed-off-by: Steve Peters Co-authored-by: Steve Peters * Make Heightmap loader use spherical coordinates. (#3258) Make Physics.cc and Gazebo GUI use spherical coordinates when loading terrains. Signed-off-by: Aditya Co-authored-by: Steve Peters --- gazebo/common/Dem.cc | 41 ++++++-- gazebo/common/Dem.hh | 7 ++ gazebo/common/DemPrivate.hh | 5 + gazebo/common/Dem_TEST.cc | 28 ++++++ gazebo/common/HeightmapData.cc | 53 ++++++++++- gazebo/common/HeightmapData.hh | 35 ++++++- gazebo/common/HeightmapData_TEST.cc | 33 +++++++ gazebo/common/SphericalCoordinates.cc | 90 ++++++++++++++++++ gazebo/common/SphericalCoordinates.hh | 40 +++++++- gazebo/common/SphericalCoordinates_TEST.cc | 103 ++++++++++++++++++++- gazebo/physics/HeightmapShape.cc | 8 +- gazebo/physics/World.cc | 30 ++++++ gazebo/physics/World.hh | 6 ++ gazebo/rendering/Heightmap.cc | 33 ++++++- test/data/dem_moon.tif | Bin 0 -> 4987 bytes 15 files changed, 495 insertions(+), 17 deletions(-) create mode 100644 test/data/dem_moon.tif diff --git a/gazebo/common/Dem.cc b/gazebo/common/Dem.cc index cc2622a864..df679201b7 100644 --- a/gazebo/common/Dem.cc +++ b/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,7 +229,8 @@ 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 @@ -229,7 +238,27 @@ void Dem::GetGeoReference(double _x, double _y, #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) { diff --git a/gazebo/common/Dem.hh b/gazebo/common/Dem.hh index e96f08c377..1957a2b270 100644 --- a/gazebo/common/Dem.hh +++ b/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. diff --git a/gazebo/common/DemPrivate.hh b/gazebo/common/DemPrivate.hh index 1a00f9b4af..17465e3eef 100644 --- a/gazebo/common/DemPrivate.hh +++ b/gazebo/common/DemPrivate.hh @@ -18,6 +18,7 @@ #ifndef _GAZEBO_DEM_PRIVATE_HH_ #define _GAZEBO_DEM_PRIVATE_HH_ +#include "gazebo/common/SphericalCoordinates.hh" #include #include @@ -59,6 +60,10 @@ namespace gazebo /// \brief DEM data converted to be OGRE-compatible. public: std::vector demData; + + /// \brief Holds the spherical coordinates object from the world. + public: common::SphericalCoordinatesPtr sphericalCoordinates = + boost::make_shared(); }; /// \} } diff --git a/gazebo/common/Dem_TEST.cc b/gazebo/common/Dem_TEST.cc index 3655a33cb9..810cc9c72e 100644 --- a/gazebo/common/Dem_TEST.cc +++ b/gazebo/common/Dem_TEST.cc @@ -21,6 +21,7 @@ #include #include +#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::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 ///////////////////////////////////////////////// diff --git a/gazebo/common/HeightmapData.cc b/gazebo/common/HeightmapData.cc index 903821cb54..39449936c6 100644 --- a/gazebo/common/HeightmapData.cc +++ b/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()); +} + +////////////////////////////////////////////////// +/// 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()); +} + +////////////////////////////////////////////////// +/// 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 diff --git a/gazebo/common/HeightmapData.hh b/gazebo/common/HeightmapData.hh index 982d2d34a5..d968b96b9e 100644 --- a/gazebo/common/HeightmapData.hh +++ b/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); }; diff --git a/gazebo/common/HeightmapData_TEST.cc b/gazebo/common/HeightmapData_TEST.cc index e43edd8a59..1c77c668d1 100644 --- a/gazebo/common/HeightmapData_TEST.cc +++ b/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(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::MOON_SCS)); + EXPECT_TRUE(heightmapData != nullptr); + + dem = dynamic_cast(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 ///////////////////////////////////////////////// diff --git a/gazebo/common/SphericalCoordinates.cc b/gazebo/common/SphericalCoordinates.cc index 00b5d9f299..025986e26e 100644 --- a/gazebo/common/SphericalCoordinates.cc +++ b/gazebo/common/SphericalCoordinates.cc @@ -44,12 +44,30 @@ const double g_EarthWGS84Flattening = 1.0/298.257223563; // Radius of the Earth (meters). const double g_EarthRadius = 6371000.0; +// Radius of the Moon (meters). +// Source: https://lunar.gsfc.nasa.gov/library/451-SCI-000958.pdf +const double g_MoonRadius = 1737400.0; + +// a: Equatorial radius of the Moon. +// Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html +const double g_MoonAxisEquatorial = 1738100.0; + +// b: Polar radius of the Moon. +// Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html +const double g_MoonAxisPolar = 1736000.0; + +// if: Unitless flattening parameter for the Moon. +// Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html +const double g_MoonFlattening = 0.0012; + ////////////////////////////////////////////////// SphericalCoordinates::SurfaceType SphericalCoordinates::Convert( const std::string &_str) { if ("EARTH_WGS84" == _str) return EARTH_WGS84; + else if ("MOON_SCS" == _str) + return MOON_SCS; gzerr << "SurfaceType string not recognized, " << "EARTH_WGS84 returned by default" << std::endl; @@ -148,6 +166,30 @@ void SphericalCoordinates::SetSurfaceType(const SurfaceType &_type) // Set the flattening parameter this->dataPtr->ellF = g_EarthWGS84Flattening; + // Set the first eccentricity ellipse parameter + // https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses + this->dataPtr->ellE = sqrt(1.0 - + std::pow(this->dataPtr->ellB, 2) / std::pow(this->dataPtr->ellA, 2)); + + // Set the second eccentricity ellipse parameter + // https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses + this->dataPtr->ellP = sqrt( + std::pow(this->dataPtr->ellA, 2) / std::pow(this->dataPtr->ellB, 2) - + 1.0); + + break; + } + case MOON_SCS: + { + // Set the semi-major axis + this->dataPtr->ellA = g_MoonAxisEquatorial; + + // Set the semi-minor axis + this->dataPtr->ellB = g_MoonAxisPolar; + + // Set the flattening parameter + this->dataPtr->ellF = g_MoonFlattening; + // Set the first eccentricity ellipse parameter // https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses this->dataPtr->ellE = sqrt(1.0 - @@ -251,6 +293,54 @@ double SphericalCoordinates::Distance(const ignition::math::Angle &_latA, return d; } +////////////////////////////////////////////////// +/// Based on Haversine formula (http://en.wikipedia.org/wiki/Haversine_formula). +/// This takes into account the surface type. +double SphericalCoordinates::DistanceBetweenPoints( + const ignition::math::Angle &_latA, + const ignition::math::Angle &_lonA, + const ignition::math::Angle &_latB, + const ignition::math::Angle &_lonB) +{ + ignition::math::Angle dLat = _latB - _latA; + ignition::math::Angle dLon = _lonB - _lonA; + + double a = sin(dLat.Radian() / 2) * sin(dLat.Radian() / 2) + + sin(dLon.Radian() / 2) * sin(dLon.Radian() / 2) * + cos(_latA.Radian()) * cos(_latB.Radian()); + + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + return this->SurfaceRadius() * c; +} + +////////////////////////////////////////////////// +double SphericalCoordinates::SurfaceRadius() const +{ + if (this->dataPtr->surfaceType == + SphericalCoordinates::SurfaceType::MOON_SCS) + return g_MoonRadius; + else + return g_EarthRadius; +} + +////////////////////////////////////////////////// +double SphericalCoordinates::SurfaceAxisEquatorial() const +{ + return this->dataPtr->ellA; +} + +////////////////////////////////////////////////// +double SphericalCoordinates::SurfaceAxisPolar() const +{ + return this->dataPtr->ellB; +} + +////////////////////////////////////////////////// +double SphericalCoordinates::SurfaceFlattening() const +{ + return this->dataPtr->ellF; +} + ////////////////////////////////////////////////// void SphericalCoordinates::UpdateTransformationMatrix() { diff --git a/gazebo/common/SphericalCoordinates.hh b/gazebo/common/SphericalCoordinates.hh index 2a649160ec..f01e3d2430 100644 --- a/gazebo/common/SphericalCoordinates.hh +++ b/gazebo/common/SphericalCoordinates.hh @@ -44,7 +44,12 @@ namespace gazebo { /// \brief Model of reference ellipsoid for earth, based on /// WGS 84 standard. see wikipedia: World_Geodetic_System - EARTH_WGS84 = 1 + EARTH_WGS84 = 1, + + /// \brief Model of the moon, based on the Selenographic + /// coordinate system, see wikipedia: Selenographic + /// Coordinate System. + MOON_SCS = 2 }; /// \enum CoordinateType @@ -119,6 +124,23 @@ namespace gazebo const ignition::math::Angle &_latB, const ignition::math::Angle &_lonB); + /// \brief Get the distance between two points expressed in geographic + /// latitude and longitude. It assumes that both points are at sea level. + /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents + /// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W. + /// This is different from the static Distance() method as it + /// takes into account the set surface's radius. + /// \param[in] _latA Latitude of point A. + /// \param[in] _lonA Longitude of point A. + /// \param[in] _latB Latitude of point B. + /// \param[in] _lonB Longitude of point B. + /// \return Distance in meters. + public: double DistanceBetweenPoints( + const ignition::math::Angle &_latA, + const ignition::math::Angle &_lonA, + const ignition::math::Angle &_latB, + const ignition::math::Angle &_lonB); + /// \brief Get SurfaceType currently in use. /// \return Current SurfaceType value. public: SurfaceType GetSurfaceType() const; @@ -131,6 +153,22 @@ namespace gazebo /// \return Reference longitude. public: ignition::math::Angle LongitudeReference() const; + /// \brief Get the radius of the surface. + /// \return radius of the surface in use. + public: double SurfaceRadius() const; + + /// \brief Get the major axis of the surface. + /// \return Equatorial axis of the surface in use. + public: double SurfaceAxisEquatorial() const; + + /// \brief Get the minor axis of the surface. + /// \return Polar axis of the surface in use. + public: double SurfaceAxisPolar() const; + + /// \brief Get the flattening of the surface. + /// \return Flattening parameter of the surface in use. + public: double SurfaceFlattening() const; + /// \brief Get reference elevation in meters. /// \return Reference elevation. public: double GetElevationReference() const; diff --git a/gazebo/common/SphericalCoordinates_TEST.cc b/gazebo/common/SphericalCoordinates_TEST.cc index 9edffc0637..2e644d1167 100644 --- a/gazebo/common/SphericalCoordinates_TEST.cc +++ b/gazebo/common/SphericalCoordinates_TEST.cc @@ -29,6 +29,17 @@ class SphericalCoordinatesTest : public gazebo::testing::AutoLogFixture { }; // Test different constructors, default parameters TEST_F(SphericalCoordinatesTest, Constructor) { + // Constants + const double g_EarthWGS84AxisEquatorial = 6378137.0; + const double g_EarthWGS84AxisPolar = 6356752.314245; + const double g_EarthWGS84Flattening = 1.0/298.257223563; + const double g_EarthRadius = 6371000.0; + + const double g_MoonRadius = 1737400.0; + const double g_MoonAxisEquatorial = 1738100.0; + const double g_MoonAxisPolar = 1736000.0; + const double g_MoonFlattening = 0.0012; + // Default surface type common::SphericalCoordinates::SurfaceType st = common::SphericalCoordinates::EARTH_WGS84; @@ -41,6 +52,14 @@ TEST_F(SphericalCoordinatesTest, Constructor) EXPECT_EQ(sc.LongitudeReference(), ignition::math::Angle()); EXPECT_EQ(sc.HeadingOffset(), ignition::math::Angle()); EXPECT_NEAR(sc.GetElevationReference(), 0.0, 1e-6); + + EXPECT_NEAR(sc.SurfaceRadius(), g_EarthRadius, 1e-6); + EXPECT_NEAR(sc.SurfaceAxisEquatorial(), + g_EarthWGS84AxisEquatorial, 1e-6); + EXPECT_NEAR(sc.SurfaceAxisPolar(), + g_EarthWGS84AxisPolar, 1e-6); + EXPECT_NEAR(sc.SurfaceFlattening(), + g_EarthWGS84Flattening, 1e-6); } // SurfaceType argument, default parameters @@ -51,6 +70,14 @@ TEST_F(SphericalCoordinatesTest, Constructor) EXPECT_EQ(sc.LongitudeReference(), ignition::math::Angle()); EXPECT_EQ(sc.HeadingOffset(), ignition::math::Angle()); EXPECT_NEAR(sc.GetElevationReference(), 0.0, 1e-6); + + EXPECT_NEAR(sc.SurfaceRadius(), g_EarthRadius, 1e-6); + EXPECT_NEAR(sc.SurfaceAxisEquatorial(), + g_EarthWGS84AxisEquatorial, 1e-6); + EXPECT_NEAR(sc.SurfaceAxisPolar(), + g_EarthWGS84AxisPolar, 1e-6); + EXPECT_NEAR(sc.SurfaceFlattening(), + g_EarthWGS84Flattening, 1e-6); } // All arguments @@ -63,6 +90,57 @@ TEST_F(SphericalCoordinatesTest, Constructor) EXPECT_EQ(sc.LongitudeReference(), lon); EXPECT_EQ(sc.HeadingOffset(), heading); EXPECT_NEAR(sc.GetElevationReference(), elev, 1e-6); + + EXPECT_NEAR(sc.SurfaceRadius(), g_EarthRadius, 1e-6); + EXPECT_NEAR(sc.SurfaceAxisEquatorial(), + g_EarthWGS84AxisEquatorial, 1e-6); + EXPECT_NEAR(sc.SurfaceAxisPolar(), + g_EarthWGS84AxisPolar, 1e-6); + EXPECT_NEAR(sc.SurfaceFlattening(), + g_EarthWGS84Flattening, 1e-6); + } + + // For Moon surface + { + common::SphericalCoordinates sc( + common::SphericalCoordinates::MOON_SCS); + EXPECT_EQ(sc.GetSurfaceType(), + common::SphericalCoordinates::MOON_SCS); + EXPECT_EQ(sc.LatitudeReference(), ignition::math::Angle()); + EXPECT_EQ(sc.LongitudeReference(), ignition::math::Angle()); + EXPECT_EQ(sc.HeadingOffset(), ignition::math::Angle()); + EXPECT_NEAR(sc.GetElevationReference(), 0.0, 1e-6); + + EXPECT_NEAR(sc.SurfaceRadius(), g_MoonRadius, 1e-6); + EXPECT_NEAR(sc.SurfaceAxisEquatorial(), + g_MoonAxisEquatorial, 1e-6); + EXPECT_NEAR(sc.SurfaceAxisPolar(), + g_MoonAxisPolar, 1e-6); + EXPECT_NEAR(sc.SurfaceFlattening(), + g_MoonFlattening, 1e-6); + } + + // Moon surface with arguments + { + ignition::math::Angle lat(0.3), lon(-1.2), heading(0.5); + double elev = 354.1; + common::SphericalCoordinates sc( + common::SphericalCoordinates::MOON_SCS, + lat, lon, elev, heading); + EXPECT_EQ(sc.GetSurfaceType(), + common::SphericalCoordinates::MOON_SCS); + EXPECT_EQ(sc.LatitudeReference(), lat); + EXPECT_EQ(sc.LongitudeReference(), lon); + EXPECT_EQ(sc.HeadingOffset(), heading); + EXPECT_NEAR(sc.GetElevationReference(), elev, 1e-6); + + EXPECT_NEAR(sc.SurfaceRadius(), g_MoonRadius, 1e-6); + EXPECT_NEAR(sc.SurfaceAxisEquatorial(), + g_MoonAxisEquatorial, 1e-6); + EXPECT_NEAR(sc.SurfaceAxisPolar(), + g_MoonAxisPolar, 1e-6); + EXPECT_NEAR(sc.SurfaceFlattening(), + g_MoonFlattening, 1e-6); } } @@ -75,6 +153,10 @@ TEST_F(SphericalCoordinatesTest, Convert) common::SphericalCoordinates::EARTH_WGS84; EXPECT_EQ(common::SphericalCoordinates::Convert("EARTH_WGS84"), st); + + // For Moon surface type + st = common::SphericalCoordinates::MOON_SCS; + EXPECT_EQ(common::SphericalCoordinates::Convert("MOON_SCS"), st); } ////////////////////////////////////////////////// @@ -261,9 +343,26 @@ TEST_F(SphericalCoordinatesTest, Distance) longA.Degree(-122.249972); latB.Degree(46.124953); longB.Degree(-122.251683); - double d = common::SphericalCoordinates::Distance(latA, longA, latB, longB); - EXPECT_NEAR(14002, d, 20); + // Calculating distance using the static method. + double d1 = common::SphericalCoordinates::Distance(latA, longA, latB, longB); + EXPECT_NEAR(14002, d1, 20); + + // Using the non static method. The default surface type is EARTH_WGS84. + common::SphericalCoordinates defaultEarthSC = common::SphericalCoordinates(); + double d2 = defaultEarthSC.DistanceBetweenPoints(latA, longA, latB, longB); + EXPECT_NEAR(d1, d2, 0.1); + + common::SphericalCoordinates earthSC = common::SphericalCoordinates( + common::SphericalCoordinates::EARTH_WGS84); + double d3 = earthSC.DistanceBetweenPoints(latA, longA, latB, longB); + EXPECT_NEAR(d2, d3, 0.1); + + // Setting the surface type as Moon. + common::SphericalCoordinates moonSC = common::SphericalCoordinates( + common::SphericalCoordinates::MOON_SCS); + double d4 = moonSC.DistanceBetweenPoints(latA, longA, latB, longB); + EXPECT_NEAR(3820, d4, 5); } ///////////////////////////////////////////////// diff --git a/gazebo/physics/HeightmapShape.cc b/gazebo/physics/HeightmapShape.cc index 48f2a74829..c1c52c3afa 100644 --- a/gazebo/physics/HeightmapShape.cc +++ b/gazebo/physics/HeightmapShape.cc @@ -81,7 +81,11 @@ void HeightmapShape::OnRequest(ConstRequestPtr &_msg) ////////////////////////////////////////////////// int HeightmapShape::LoadTerrainFile(const std::string &_filename) { - this->heightmapData = common::HeightmapDataLoader::LoadTerrainFile(_filename); + common::SphericalCoordinatesPtr sphericalCoordinates; + sphericalCoordinates = this->world->SphericalCoords(); + + this->heightmapData = common::HeightmapDataLoader::LoadTerrainFile(_filename, + sphericalCoordinates); if (!this->heightmapData) { gzerr << "Unable to load heightmap data" << std::endl; @@ -108,8 +112,6 @@ int HeightmapShape::LoadTerrainFile(const std::string &_filename) // Modify the reference geotedic latitude/longitude. // A GPS sensor will use the real georeferenced coordinates of the terrain. - common::SphericalCoordinatesPtr sphericalCoordinates; - sphericalCoordinates = this->world->SphericalCoords(); if (sphericalCoordinates) { diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index b0f1ca6194..f64078424e 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -393,9 +393,18 @@ void World::Load(sdf::ElementPtr _sdf) surfaceType, latitude, longitude, elevation, heading)); } + if (this->dataPtr->sphericalCoordinates == nullptr) gzthrow("Unable to create spherical coordinates data structure\n"); + std::string sphericalCoordinatesSurfaceService("/spherical_coordinates_surface_type"); + if (!this->dataPtr->ignNode.Advertise(sphericalCoordinatesSurfaceService, + &World::SphericalCoordinatesSurfaceService, this)) + { + gzerr << "Error advertising service [" << + sphericalCoordinatesSurfaceService << "]" << std::endl; + } + this->dataPtr->rootElement.reset(new Base(BasePtr())); this->dataPtr->rootElement->SetName(this->Name()); this->dataPtr->rootElement->SetWorld(shared_from_this()); @@ -3439,6 +3448,27 @@ bool World::ShadowCasterMaterialNameService(ignition::msgs::StringMsg &_res) return true; } +////////////////////////////////////////////////// +bool World::SphericalCoordinatesSurfaceService(ignition::msgs::StringMsg &_res) +{ + if (this->dataPtr->sphericalCoordinates != nullptr) + { + if (this->dataPtr->sphericalCoordinates->GetSurfaceType() + == common::SphericalCoordinates::MOON_SCS) + { + _res.set_data("MOON_SCS"); + return true; + } + if (this->dataPtr->sphericalCoordinates->GetSurfaceType() + == common::SphericalCoordinates::EARTH_WGS84) + { + _res.set_data("EARTH_WGS84"); + return true; + } + } + return false; +} + ////////////////////////////////////////////////// bool World::ShadowCasterRenderBackFacesService(ignition::msgs::Boolean &_res) { diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh index 5e709ce8d7..3070cdb5c5 100644 --- a/gazebo/physics/World.hh +++ b/gazebo/physics/World.hh @@ -669,6 +669,12 @@ namespace gazebo private: bool ShadowCasterMaterialNameService( ignition::msgs::StringMsg &_response); + /// \brief Callback for "/spherical_coordinates_surface_type" service. + /// \param[out] _response Message containing spherical coordinates surface type. + /// \return True if the info was successfully obtained. + public: bool SphericalCoordinatesSurfaceService( + ignition::msgs::StringMsg &_response); + /// \brief Callback for "/shadow_caster_render_back_faces" /// service. /// \param[out] _response Message containing shadow caster render back diff --git a/gazebo/rendering/Heightmap.cc b/gazebo/rendering/Heightmap.cc index 4ef300709a..860d41d06e 100644 --- a/gazebo/rendering/Heightmap.cc +++ b/gazebo/rendering/Heightmap.cc @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -395,8 +396,38 @@ void Heightmap::Load() // try loading heightmap data locally if (!this->dataPtr->filename.empty()) { + // Get spherical coordinates surface name from physics::World + std::string surfaceType = "EARTH_WGS84"; + { + ignition::transport::Node node; + ignition::msgs::StringMsg rep; + const std::string serviceName = "/spherical_coordinates_surface_type"; + bool result; + unsigned int timeout = 5000; + bool executed = node.Request(serviceName, + timeout, rep, result); + if (executed) + { + if (result) + surfaceType = rep.data(); + else + gzerr << "Service call[" << serviceName << "] failed" << std::endl; + } + else + { + gzerr << "Service call[" << serviceName << "] timed out" << std::endl; + } + } + + common::SphericalCoordinatesPtr sphericalCoordinates; + if (surfaceType == "MOON_SCS") + { + sphericalCoordinates = boost::make_shared( + common::SphericalCoordinates::MOON_SCS); + } + this->dataPtr->heightmapData = common::HeightmapDataLoader::LoadTerrainFile( - this->dataPtr->filename); + this->dataPtr->filename, sphericalCoordinates); if (this->dataPtr->heightmapData) { diff --git a/test/data/dem_moon.tif b/test/data/dem_moon.tif new file mode 100644 index 0000000000000000000000000000000000000000..955f14da88227b70ae82a61bcf6ddbaee6607989 GIT binary patch literal 4987 zcmaKvcXUM_TDK3-I}| zD#`GBZteeUU4r`o&(@jG1wFUU=NFju-M;=lU)Xc^!=J4)f3wiDX9Dy2d_|voChFNb zfM;?K3-x8r2N^?CLw!Na7Z@wj0(>o)!`XOcT4)yHU92D~YT|Mpr}wr4(3``V?fIdkoo^)>~4zQCtzpl99l56no*llcIY`T3p<@(a#L zYwyeB^M7_7lr@hl9sKM%BzR?-pD&v)N7fwV%at`}_xVDfe&7Gu9FR5V@dakh6-tMM zW`57-%aJt)_;O~=fxd!Sv%jxU)|~mobAN+Bs{Py*%bV~2-|MGW8-~97LFVdzbLQIX z(IcTs<;s;~5_!kb-#xZO%@QnQGCxhO*Z$Mq-MNu@5tI3Ba{c)DxSqXw z#C3Z9ZcO73K4dgZc1dyFV>7=H8QVEFDK?>FY>XG16xS)PeL`}wuaUn|yC9=FAx6RZ zjO-Pojy|gb{#L`otb%VFiR-X%t1B;AednwuCRm*sV)b~ARl2N(p0&F3x7Ck@oT|L% zl%uWFgLtP_L!E9@f2=Pw2tEz53H&d=!kaHI4~ zMhpHn8kEgya3QPl_l^9)&@aE$qEcWpR-59jE|0JpJZeY-M>~DXzE^rUt?%nJXNc3jbf*acMz3Zwnt999;<`qs z&lrXLWAyo5qeS-Ze#>Y~cB>h=!Bg03Rb#6v9jwCpTa}w)mAa1mC#>E8!|;4gTTfX{ zZtWBVh8BaIJ^)Xy7;vRJeLdD`bF$N)2~M$$W$?0Qt5a2YcsGyH&ay_gzc89{j`b6x zFK!rhxokA~PorHqtjtYYf3s?F-)j3yPW|3= zTGh~Lc~hsU9od`rPX@!A#NnG3PFuP-wVCB~^9QHEUPj3;8lCTLbm?28UVj+P=G`B{ z#XPvEPAsxNHu7>=*nD6uv zT710PDgVEnPWCjK)59nYEQh8T4a`6nyN$9PGabxM)$p>gUSDhaBy;g!lfp z)s1~tr*GpUrJU|paQdvMQ_gBm=ihef500PUAQe3xT;LRg4x65Ka%Y{+fumpw8jLfV zJHqG~JbXLWsL@wOd%rX~1s}tHHwxsv1IW{`dq$D?$L3;I_nKMFO1A1a)2byH+7gRR zU}%%Wsr<`MB^d|cKe4P+2z+!a=afCp>C{rz`<%SPaP_@Y_dQO#J~Qe%#OTTZqvPQD zZ4i5mH2QI((F@>sXPeR3V@8+JN5N}GEy;(P#qrE0R>_0OM=*pgL%Y9Q-6t}Q+ zs?*UDPOZ_!hybVkTn}!7KRWWFC;7Ss@7nBib&*s16;8*{!GgXy+`SFRsSB(~fp#{45XPD6i zI9ZGi=WQ@b`PFFqX`|+N*ABE=ysK5AudU`Du^NO={_+;TixE z6sZNpqtyF&q>>&)XaYJ2j&>?N8o%6#ujl3Wa~riOV6-(KxlqRFjShIxB%_LqnrOA? zzreSVu@=36Wz;V`cby!09e+x+T8%H2D&jP*y3^WvPSqY;ohU;LdPV5kkw}eR6Q%PZ z(JFB)N)JP$)Vy7U=Jj!EjGv!e;53?gH7eL>9V0xzD6XK<2}gdQg%)72dC>e*JSY*om;_nqd$#a(oezZM*yF`0(;s^3a@*6RN1n1#JBxs!Z$sjbZ}nqt28Q3r*_>UBayqe|$a9(-(S52j9VwS^i!-f9f-Z?Y22 zQop8q)T>!&50C1IM>j-^hFY{5J+vS<+Z2t^#`=*OTp&tE$3-coca%14i_~kKBDAnQ z9!mXwbq#$5F=#+M(sQy;m{C6VKf-lOcv-}H0GKj-MrFyLt<~sZ>RMgjil>BHHIKIP zqfRv6V|CV_{KJnfb#ppI4h{s*zBW#gZ}HB7k@}=vq&}`1sTw^aba|Z99W?n4b^OV4 zr|*NP72vo6hV&<%R-wyR&Uw0X)>G}Pp3;Bt^nm-b$&;JpLsSrXu)wG!UXesE(J~dk z!lSBProUp}$IXc|UNrY3r{b>@>$*-g;rq}bJbIf`ibu^XNDtG&Y2D}e7+zh5->*zO zZt$+jzo3hsJyqH7Y4~nWOZIwd^B+$qj~M+#pAtqLF4Ygce`EFVC+c|xtA%i}mb$s{ z7kU}|a6%oYuI-#^u~&FrxGwATB_6fdIxTI0H>24{?>dbI=ldh*=g809-+H>f)>E&Q zo&sig>NelgA4@!S-sCB9y{FjfXb~@5zsadYu?Q6?9HG*QP7P~V?Ij+aiO(|pauJ%C zlE-O0wW)ezr;F5xa}}K?R>HUIIdwsUrw2IgY2{P{u2$h)CsV*f9o;<9)7R5Hy*|ZL z={}xb9_%SJ9bBtDRR=@U59x2dBj&&2@h(DMjR;L!g9*T zGV}ir+IMi zLzCm_C-<|@g&t0Ce~LGP;rFiea$tCp1Q)UNcB$xL1pD#+#m9IGoaO20E>ATt5*Kn~ zK029+S6&{)o_KNwx~SfX{Gg5%_GjO`R^Ng#5se(aVzs=mQ(JoL=I|dCO`f2OFTgSp z{#SCnt)tT!YSjVaFmjC3&uC)ePG_#t6E{7kb&kZ5&VNMN-(IcRVYIs$NhV)KR)U!yZ?ew#E z+SAvMV^GJ-taF+H2QkE<=xlOfpQnfSJYT^cMb@hdAuK z%^D1o-lSgA&pha0rA2r&y^Ot1AC6b1mUepi1-yz{(hM(ei%ycsjbcrm>ZPL7mGl}1 z@OU&?Yq+PR(VkvB;OV6Yp7P*P$I4Pe80~7ZZb^-RgGJP;I`IEdeybO$-KU>e{mS^U z4|;sX>Ql6Na}E8>NvneN5~ZkJ-H2c9T27M&;Tdo+wgNr&duU*|(}wjT=tZL24bk6{mN0JtKdZz9~+N9Q!8rGQ?;Y#4W(yTj>c-YbGBz%&-W*tiJ~pX~H8dP*%v%_P6mS5V_laL!3#Z~P!#o;v>F z>Du3(+T)$a!i?@y(?{eoS^^ip_eKZb7(GCX>(i*;HlBTk>+Ns^e>Z-xT3Uws)*sDmz$>ZS;b^chHM~Ap4l|yR6C3i=$I(|M)TV~D zg8RWnN9iZZqsx?Kc=tfg1C!|?CeZVN?KUyG^c`m-ymM-pRl9(v=k(k)IsXu&NoXa0 zD*Ychod1+n06e@gikkT!e3SS$1VeZBj=S#Z2YBcN|3}D+402*XANscuoO`}9T0Wg~ z*jmPF2Krdqi?dEMdc8&T5>2c^cEQh1K0~`Z;H$t{&aC;Yjy1BHmSk0V7-xg6a1g-R zi=4fMPP^{Lf2og2)QEp3Q5&d{>&S}}p+>vGu=*2XkZv^n1U=_1dR}^`?Q=Qfkf$r* z#Fu1bYS4dGqbH9?GxdzV>tgi!0Afh*vyo?ypMgK}x56iAB#nLt47qOO@5EvdK5pha F{TC%IGC2SM literal 0 HcmV?d00001