void NavSatTransform::odomCallback(const nav_msgs::OdometryConstPtr& msg)
{
if(!transformGood_ && !useManualDatum_)
{
setTransformOdometry(msg);
}
tf2::fromMsg(msg->pose.pose, latestWorldPose_);
odomUpdateTime_ = msg->header.stamp;
odomUpdated_ = true;
}
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;
}