enu.cpp

Go to the documentation of this file.

#include "enu/enu.h"

#include <boost/bind.hpp>
#include <string>

extern "C" {
  #include "libswiftnav/coord_system.h"
}

#include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h"
#include "geometry_msgs/Point.h"

#define TO_RADIANS (M_PI/180)
#define TO_DEGREES (180/M_PI)


extern "C" int xerbla_(char *srname, int *info)
{
  ROS_ERROR("On entry to %6s, parameter number %2i had an illegal value", srname, *info);
}

namespace enu {

static void fix_to_ecef(const sensor_msgs::NavSatFix& fix, double ecef[3]) {
  double llh[3] = { fix.latitude * TO_RADIANS,
                    fix.longitude * TO_RADIANS,
                    fix.altitude };
  wgsllh2ecef(llh, ecef);
}

void fix_to_point(const sensor_msgs::NavSatFix& fix,
                  const sensor_msgs::NavSatFix& datum,
                  geometry_msgs::Point* point_ptr) {
  // Convert reference LLH-formatted datum to ECEF format
  double ecef_datum[3];
  fix_to_ecef(datum, ecef_datum);

  // Prepare the appropriate input vector to convert the input latlon
  // to an ECEF triplet.
  double llh[3] = { fix.latitude * TO_RADIANS,
                    fix.longitude * TO_RADIANS,
                    fix.altitude };
  double ecef[3];
  wgsllh2ecef(llh, ecef);

  // ECEF triplet is converted to north-east-down (NED), by combining it
  // with the ECEF-formatted datum point.
  double ned[3];
  wgsecef2ned_d(ecef, ecef_datum, ned);

  // Output data
  point_ptr->x = ned[1];
  point_ptr->y = ned[0];
  point_ptr->z = -ned[2];
}

void point_to_fix(const geometry_msgs::Point& point,
                  const sensor_msgs::NavSatFix& datum,
                  sensor_msgs::NavSatFix* fix_ptr) {
  // Convert reference LLH-formatted datum to ECEF format
  double ecef_datum[3];
  fix_to_ecef(datum, ecef_datum);

  // Prepare NED vector from ENU coordinates, perform conversion in libswiftnav
  // library calls.
  double ned[3] = { point.y, point.x, -point.z };

  double ecef[3];
  wgsned2ecef_d(ned, ecef_datum, ecef);

  double llh_raw[3];
  wgsecef2llh(ecef, llh_raw);

  // Output Fix message. Convert radian latlon output back to degrees.
  fix_ptr->latitude = llh_raw[0] * TO_DEGREES;
  fix_ptr->longitude = llh_raw[1] * TO_DEGREES;
  fix_ptr->altitude = llh_raw[2];
}

}  // namespace enu