Bug in SphericalCoordinates::SphericalFromLocal?

asked 2016-07-04 14:58:51 -0600

laughlin gravatar image

updated 2016-07-05 14:37:31 -0600

I was under the impression that gazebo assumed an East, North, Up (ENU) right-handed frame for the SphericalCoordinate class, but I think there's a bug in the conversion for lat-long.

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 believe the offending negation is here.

edit retag flag offensive close merge delete