Skip to content

Commit

Permalink
replaced exceptions with errors
Browse files Browse the repository at this point in the history
Signed-off-by: Jenn Nguyen <[email protected]>
  • Loading branch information
jennuine committed Nov 19, 2021
1 parent ae763a0 commit 6f6769a
Show file tree
Hide file tree
Showing 3 changed files with 47 additions and 45 deletions.
9 changes: 6 additions & 3 deletions geospatial/include/ignition/common/Dem.hh
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@ namespace ignition
/// \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.
/// \return Terrain's elevation at (x,y) in meters.
/// \return Terrain's elevation at (x,y) in meters or infinity if illegal
/// coordinates were provided.
public: double Elevation(double _x, double _y);

/// \brief Get the terrain's minimum elevation in meters.
Expand All @@ -67,7 +68,8 @@ namespace ignition
/// origin in WGS84.
/// \param[out] _latitude Georeferenced latitude.
/// \param[out] _longitude Georeferenced longitude.
public: void GeoReferenceOrigin(ignition::math::Angle &_latitude,
/// \return True if able to retrieve origin coordinates. False otherwise.
public: bool GeoReferenceOrigin(ignition::math::Angle &_latitude,
ignition::math::Angle &_longitude) const;

/// \brief Get the terrain's height. Due to the Ogre constrains, this
Expand Down Expand Up @@ -119,7 +121,8 @@ namespace ignition
/// \param[in] _y Y coordinate of the terrain.
/// \param[out] _latitude Georeferenced latitude.
/// \param[out] _longitude Georeferenced longitude.
private: void GeoReference(double _x, double _y,
/// \return True if able to retrieve coordinates. False otherwise.
private: bool GeoReference(double _x, double _y,
ignition::math::Angle &_latitude,
ignition::math::Angle &_longitude) const;

Expand Down
73 changes: 35 additions & 38 deletions geospatial/src/Dem.cc
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
*
*/
#include <algorithm>
#include <limits>

#include <gdal_priv.h>
#include <ogr_spatialref.h>
Expand Down Expand Up @@ -118,35 +119,32 @@ int Dem::Load(const std::string &_filename)
ySize = this->dataPtr->dataSet->GetRasterYSize();

// Corner coordinates
try
upLeftX = 0.0;
upLeftY = 0.0;
upRightX = xSize;
upRightY = 0.0;
lowLeftX = 0.0;
lowLeftY = ySize;

// Calculate the georeferenced coordinates of the terrain corners
if (!this->GeoReference(upLeftX, upLeftY, upLeftLat, upLeftLong)
|| !this->GeoReference(upRightX, upRightY, upRightLat, upRightLong)
|| !this->GeoReference(lowLeftX, lowLeftY, lowLeftLat, lowLeftLong))
{
upLeftX = 0.0;
upLeftY = 0.0;
upRightX = xSize;
upRightY = 0.0;
lowLeftX = 0.0;
lowLeftY = ySize;

// Calculate the georeferenced coordinates of the terrain corners
this->GeoReference(upLeftX, upLeftY, upLeftLat, upLeftLong);
this->GeoReference(upRightX, upRightY, upRightLat, upRightLong);
this->GeoReference(lowLeftX, lowLeftY, lowLeftLat, lowLeftLong);

// Set the world width and height
this->dataPtr->worldWidth =
math::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
upRightLat, upRightLong);
this->dataPtr->worldHeight =
math::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
lowLeftLat, lowLeftLong);
}
catch (const std::invalid_argument &)
{
ignwarn << "Failed to automatically compute DEM size. "
ignerr << "Failed to automatically compute DEM size. "
<< "Please use the <size> element to manually set DEM size."
<< std::endl;
return -1;
}

// Set the world width and height
this->dataPtr->worldWidth =
math::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
upRightLat, upRightLong);
this->dataPtr->worldHeight =
math::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
lowLeftLat, lowLeftLong);

// Set the terrain's side (the terrain will be squared after the padding)
if (ignition::math::isPowerOfTwo(ySize - 1))
height = ySize;
Expand Down Expand Up @@ -201,11 +199,10 @@ double Dem::Elevation(double _x, double _y)
{
if (_x >= this->Width() || _y >= this->Height())
{
throw std::invalid_argument(
"Illegal coordinates. You are asking for the elevation in ("
+ std::to_string(_x) + "," + std::to_string(_y) + ") but the terrain is ["
+ std::to_string(this->Width()) + " x " + std::to_string(this->Height())
+ "]\n");
ignerr << "Illegal coordinates. You are asking for the elevation in ("
<< _x << "," << _y << ") but the terrain is ["
<< this->Width() << " x " << this->Height() << "]" << std::endl;
return std::numeric_limits<double>::infinity();
}

return this->dataPtr->demData.at(_y * this->Width() + _x);
Expand All @@ -224,7 +221,7 @@ float Dem::MaxElevation() const
}

//////////////////////////////////////////////////
void Dem::GeoReference(double _x, double _y,
bool Dem::GeoReference(double _x, double _y,
ignition::math::Angle &_latitude, ignition::math::Angle &_longitude) const
{
double geoTransf[6];
Expand All @@ -243,10 +240,9 @@ void Dem::GeoReference(double _x, double _y,
cT = OGRCreateCoordinateTransformation(&sourceCs, &targetCs);
if (nullptr == cT)
{
throw std::invalid_argument(
"Unable to transform terrain coordinate system to WGS84 for "
"coordinates (" + std::to_string(_x) + ","
+ std::to_string(_y) + ")");
ignerr << "Unable to transform terrain coordinate system to WGS84 for "
<< "coordinates (" << _x << "," << _y << ")" << std::endl;
return false;
}

xGeoDeg = geoTransf[0] + _x * geoTransf[1] + _y * geoTransf[2];
Expand All @@ -261,14 +257,15 @@ void Dem::GeoReference(double _x, double _y,
}
else
{
throw std::invalid_argument(
"Unable to obtain the georeferenced values for coordinates ("
+ std::to_string(_x) + "," + std::to_string(_y) + ")\n");
ignerr << "Unable to obtain the georeferenced values for coordinates ("
<< _x << "," << _y << ")" << std::endl;
return false;
}
return true;
}

//////////////////////////////////////////////////
void Dem::GeoReferenceOrigin(ignition::math::Angle &_latitude,
bool Dem::GeoReferenceOrigin(ignition::math::Angle &_latitude,
ignition::math::Angle &_longitude) const
{
return this->GeoReference(0, 0, _latitude, _longitude);
Expand Down
10 changes: 6 additions & 4 deletions geospatial/src/Dem_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
*/

#include <gtest/gtest.h>
#include <limits>
#include <ignition/math/Angle.hh>
#include <ignition/math/Vector3.hh>

Expand Down Expand Up @@ -101,13 +102,14 @@ TEST_F(DemTest, BasicAPI)
EXPECT_FLOAT_EQ(209.14784, dem.Elevation(width - 1, height - 1));

// Illegal coordinates
ASSERT_ANY_THROW(dem.Elevation(0, height));
ASSERT_ANY_THROW(dem.Elevation(width, 0));
ASSERT_ANY_THROW(dem.Elevation(width, height));
double inf = std::numeric_limits<double>::infinity();
EXPECT_DOUBLE_EQ(inf, dem.Elevation(0, height));
EXPECT_DOUBLE_EQ(inf, dem.Elevation(width, 0));
EXPECT_DOUBLE_EQ(inf, dem.Elevation(width, height));

// Check GeoReferenceOrigin()
ignition::math::Angle latitude, longitude;
dem.GeoReferenceOrigin(latitude, longitude);
EXPECT_TRUE(dem.GeoReferenceOrigin(latitude, longitude));
EXPECT_FLOAT_EQ(38.001667, latitude.Degree());
EXPECT_FLOAT_EQ(-122.22278, longitude.Degree());
}
Expand Down

0 comments on commit 6f6769a

Please sign in to comment.