void NavSatTransform::gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg)
{
hasGps_ = (msg->status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX &&
!std::isnan(msg->altitude) &&
!std::isnan(msg->latitude) &&
!std::isnan(msg->longitude));
if(hasGps_)
{
double utmX = 0;
double utmY = 0;
gps_common::LLtoUTM(msg->latitude, msg->longitude, utmY, utmX, utmZone_);
latestUtmPose_.setOrigin(tf::Vector3(utmX, utmY, msg->altitude));
latestUtmCovariance_.setZero();
// Copy the measurement's covariance matrix so that we can rotate it later
for (size_t i = 0; i < POSITION_SIZE; i++)
{
for (size_t j = 0; j < POSITION_SIZE; j++)
{
latestUtmCovariance_(i, j) = msg->position_covariance[POSITION_SIZE * i + j];
}
}
gpsUpdateTime_ = msg->header.stamp;
gpsUpdated_ = true;
}
}
bool NavSatTransform::prepareFilteredGps(sensor_msgs::NavSatFix &filteredGps)
{
bool newData = false;
if (transformGood_ && odomUpdated_)
{
tf2::Transform odomAsUtm;
odomAsUtm.mult(utmWorldTransInverse_, latestWorldPose_);
odomAsUtm.setRotation(tf2::Quaternion::getIdentity());
// Rotate the covariance as well
tf2::Matrix3x3 rot(utmWorldTransInverse_.getRotation());
Eigen::MatrixXd rot6d(POSE_SIZE, POSE_SIZE);
rot6d.setIdentity();
for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd)
{
rot6d(rInd, 0) = rot.getRow(rInd).getX();
rot6d(rInd, 1) = rot.getRow(rInd).getY();
rot6d(rInd, 2) = rot.getRow(rInd).getZ();
rot6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
rot6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
rot6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
}
// Rotate the covariance
latestOdomCovariance_ = rot6d * latestOdomCovariance_.eval() * rot6d.transpose();
// Now convert the data back to lat/long and place into the message
NavsatConversions::UTMtoLL(odomAsUtm.getOrigin().getY(),
odomAsUtm.getOrigin().getX(),
utmZone_,
filteredGps.latitude,
filteredGps.longitude);
filteredGps.altitude = odomAsUtm.getOrigin().getZ();
// Copy the measurement's covariance matrix back
for (size_t i = 0; i < POSITION_SIZE; i++)
{
for (size_t j = 0; j < POSITION_SIZE; j++)
{
filteredGps.position_covariance[POSITION_SIZE * i + j] = latestUtmCovariance_(i, j);
}
}
filteredGps.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
filteredGps.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
filteredGps.header.frame_id = "gps";
filteredGps.header.stamp = odomUpdateTime_;
// Mark this GPS as used
odomUpdated_ = false;
newData = true;
}
return newData;
}
bool NavSatTransform::prepareGpsOdometry(nav_msgs::Odometry &gpsOdom)
{
bool newData = false;
if (transformGood_ && gpsUpdated_)
{
tf2::Transform transformedUtm;
transformedUtm.mult(utmWorldTransform_, latestUtmPose_);
transformedUtm.setRotation(tf2::Quaternion::getIdentity());
// Rotate the covariance as well
tf2::Matrix3x3 rot(utmWorldTransform_.getRotation());
Eigen::MatrixXd rot6d(POSE_SIZE, POSE_SIZE);
rot6d.setIdentity();
for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd)
{
rot6d(rInd, 0) = rot.getRow(rInd).getX();
rot6d(rInd, 1) = rot.getRow(rInd).getY();
rot6d(rInd, 2) = rot.getRow(rInd).getZ();
rot6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
rot6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
rot6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
}
// Rotate the covariance
latestUtmCovariance_ = rot6d * latestUtmCovariance_.eval() * rot6d.transpose();
// Now fill out the message. Set the orientation to the identity.
tf2::toMsg(transformedUtm, gpsOdom.pose.pose);
gpsOdom.pose.pose.position.z = (zeroAltitude_ ? 0.0 : gpsOdom.pose.pose.position.z);
// Copy the measurement's covariance matrix so that we can rotate it later
for (size_t i = 0; i < POSE_SIZE; i++)
{
for (size_t j = 0; j < POSE_SIZE; j++)
{
gpsOdom.pose.covariance[POSE_SIZE * i + j] = latestUtmCovariance_(i, j);
}
}
gpsOdom.header.frame_id = worldFrameId_;
gpsOdom.header.stamp = gpsUpdateTime_;
gpsOdom.child_frame_id="base_link";
// Mark this GPS as used
gpsUpdated_ = false;
newData = true;
}
return newData;
}
void NavSatTransform::gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg)
{
// Make sure the GPS data is usable
bool goodGps = (msg->status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX &&
!std::isnan(msg->altitude) &&
!std::isnan(msg->latitude) &&
!std::isnan(msg->longitude));
if(goodGps)
{
// If we haven't computed the transform yet, then
// store this message as the initial GPS data to use
if(!transformGood_ && !useManualDatum_)
{
setTransformGps(msg);
}
double utmX = 0;
double utmY = 0;
std::string utmZoneTmp;
NavsatConversions::LLtoUTM(msg->latitude, msg->longitude, utmY, utmX, utmZoneTmp);
latestUtmPose_.setOrigin(tf2::Vector3(utmX, utmY, msg->altitude));
latestUtmCovariance_.setZero();
// Copy the measurement's covariance matrix so that we can rotate it later
for (size_t i = 0; i < POSITION_SIZE; i++)
{
for (size_t j = 0; j < POSITION_SIZE; j++)
{
latestUtmCovariance_(i, j) = msg->position_covariance[POSITION_SIZE * i + j];
}
}
gpsUpdateTime_ = msg->header.stamp;
gpsUpdated_ = true;
}
}
