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 0000000000..955f14da88 Binary files /dev/null and b/test/data/dem_moon.tif differ