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);
}
Datum* create(const char * classname) {
Datum *self;
Newx(self,1,Datum);
initialize_datum(self);
return self;
}
