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]); } }