bool NavSatTransform::datumCallback(robot_localization::SetDatum::Request& request,
robot_localization::SetDatum::Response&)
{
transformGood_ = false;
sensor_msgs::NavSatFix *fix = new sensor_msgs::NavSatFix();
fix->latitude = request.geo_pose.position.latitude;
fix->longitude = request.geo_pose.position.longitude;
fix->altitude = request.geo_pose.position.altitude;
fix->header.stamp = ros::Time::now();
fix->position_covariance[0] = 0.1;
fix->position_covariance[4] = 0.1;
fix->position_covariance[8] = 0.1;
fix->position_covariance_type = sensor_msgs::NavSatStatus::STATUS_FIX;
sensor_msgs::NavSatFixConstPtr fixPtr(fix);
setTransformGps(fixPtr);
nav_msgs::Odometry *odom = new nav_msgs::Odometry();
odom->pose.pose.orientation.x = 0;
odom->pose.pose.orientation.y = 0;
odom->pose.pose.orientation.z = 0;
odom->pose.pose.orientation.w = 1;
odom->pose.pose.position.x = 0;
odom->pose.pose.position.y = 0;
odom->pose.pose.position.z = 0;
odom->header.frame_id = worldFrameId_;
odom->child_frame_id = baseLinkFrameId_;
nav_msgs::OdometryConstPtr odomPtr(odom);
setTransformOdometry(odomPtr);
sensor_msgs::Imu *imu = new sensor_msgs::Imu();
imu->orientation = request.geo_pose.orientation;
imu->header.frame_id = baseLinkFrameId_;
sensor_msgs::ImuConstPtr imuPtr(imu);
imuCallback(imuPtr);
return true;
}
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;
}
}
