I was under the impression that gazebo assumed an East, North, Up (ENU) right-handed frame for the SphericalCoordinate class, but as far as I can tell, it doesn't follow that convention.
Test code:
#include <gazebo/common/common.hh>
#include <ignition/math/Angle.hh>
#include <ignition/math/Vector3.hh>
int main(){
const ignition::math::Vector3d far_x(1000, 0, 0);
const ignition::math::Vector3d far_y(0,1000, 0);
const ignition::math::Angle lat(0), lon(0), hdg(0);
double ele = 0;
gazebo::math::Vector3 lat_long;
const gazebo::common::SphericalCoordinates::SurfaceType earth = (gazebo::common::SphericalCoordinates::SurfaceType)1;
gazebo::common::SphericalCoordinates sphericalCords(earth,lat,lon,ele,hdg);
//assuming ENU coordinates, pos x should be positive longitude
lat_long = sphericalCords.SphericalFromLocal(far_x);
std::cout << "Far x: [" << far_x << "] ---> [" << lat_long << "] \n";
//positive y should be positive latitude in ENU
lat_long = sphericalCords.SphericalFromLocal(far_y);
std::cout << "Far y: [" << far_y << "] ---> [" << lat_long << "] \n";
}
with results:
Far x: [1000 0 0] ---> [0 -0.008983 0.078393]
Far y: [0 1000 0] ---> [-0.009044 0 0.078921]
The returned latitude and longitude should be positive if we're operating in an ENU coordinate system (assuming 0 deg heading offset, as is the case above).
Is this the intended behavior, or is this a bug?
I wanted to check that I'm not missing something before submitting a pull request. The offending negation is here.
