static void handle_fix(const sensor_msgs::NavSatFixConstPtr fix_ptr,
const ros::Publisher& pub_enu,
const ros::Publisher& pub_datum)
{
static double ecef_datum[3];
static bool have_datum = false;
if (!have_datum) {
initialize_datum(ecef_datum, fix_ptr, pub_datum);
have_datum = true;
}
// Prepare the appropriate input vector to convert the input latlon
// to an ECEF triplet.
double llh[3] = { fix_ptr->latitude * TO_RADIANS,
fix_ptr->longitude * TO_RADIANS,
fix_ptr->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);
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = fix_ptr->header.stamp;
odom_msg.pose.pose.position.x = ned[1];
odom_msg.pose.pose.position.y = ned[0];
odom_msg.pose.pose.position.z = -ned[2];
// We only need to populate the diagonals of the covariance matrix; the
// rest initialize to zero automatically, which is correct as the
// dimensions of the state are independent.
odom_msg.pose.covariance[0] = fix_ptr->position_covariance[0];
odom_msg.pose.covariance[7] = fix_ptr->position_covariance[4];
odom_msg.pose.covariance[14] = fix_ptr->position_covariance[8];
// Do not use/trust orientation dimensions from GPS.
odom_msg.pose.covariance[21] = 1e6;
odom_msg.pose.covariance[28] = 1e6;
odom_msg.pose.covariance[35] = 1e6;
// Orientation of GPS is pointing upward (as far as we know).
odom_msg.pose.pose.orientation.w = 1;
pub_enu.publish(odom_msg);
}
/** Determine the azimuth and elevation of a point in WGS84 Earth Centered,
* Earth Fixed (ECEF) Cartesian coordinates from a reference point given in
* WGS84 ECEF coordinates.
*
* First the vector between the points is converted into the local North, East,
* Down frame of the reference point. Then we can directly calculate the
* azimuth and elevation.
*
* \param ecef Cartesian coordinates of the point, passed as [X, Y, Z],
* all in meters.
* \param ref_ecef Cartesian coordinates of the reference point from which the
* azimuth and elevation is to be determined, passed as
* [X, Y, Z], all in meters.
* \param azimuth Pointer to where to store the calculated azimuth output.
* \param elevation Pointer to where to store the calculated elevation output.
*/
void wgsecef2azel(const double ecef[3], const double ref_ecef[3],
double* azimuth, double* elevation) {
double ned[3];
/* Calculate the vector from the reference point in the local North, East,
* Down frame of the reference point. */
wgsecef2ned_d(ecef, ref_ecef, ned);
*azimuth = atan2(ned[1], ned[0]);
/* atan2 returns angle in range [-pi, pi], usually azimuth is defined in the
* range [0, 2pi]. */
if (*azimuth < 0)
*azimuth += 2*M_PI;
*elevation = asin(-ned[2]/vector_norm(3, ned));
}
END_TEST
/* Check simply that passing the ECEF position the same as the
* reference position returns (0, 0, 0) in NED frame */
START_TEST(test_random_wgsecef2ned_d_0) {
s32 i, j;
double ned[3];
seed_rng();
for (i = 0; i < 222; i++) {
const double ecef[3] = {frand(-1e8, 1e8),
frand(-1e8, 1e8),
frand(-1e8, 1e8)};
wgsecef2ned_d(ecef, ecef, ned);
for (j = 0; j < 3; j++)
fail_unless(fabs(ned[j]) < 1e-8,
"NED vector to reference ECEF point "
"has nonzero element %d: %lf\n"
"(point was <%lf %lf %lf>)\n",
ned[j], ecef[0], ecef[1], ecef[2]);
}
}
