void NavSatTransform::setTransformOdometry(const nav_msgs::OdometryConstPtr& msg)
{
tf2::fromMsg(msg->pose.pose, transformWorldPose_);
worldFrameId_ = msg->header.frame_id;
baseLinkFrameId_ = msg->child_frame_id;
hasTransformOdom_ = true;
ROS_INFO_STREAM("Initial odometry position is (" << std::fixed <<
transformWorldPose_.getOrigin().getX() << ", " <<
transformWorldPose_.getOrigin().getY() << ", " <<
transformWorldPose_.getOrigin().getZ() << ")");
// Users can optionally use the (potentially fused) heading from
// the odometry source, which may have multiple fused sources of
// heading data, and so would act as a better heading for the
// UTM->world_frame transform.
if(!transformGood_ && useOdometryYaw_ && !useManualDatum_)
{
sensor_msgs::Imu *imu = new sensor_msgs::Imu();
imu->orientation = msg->pose.pose.orientation;
imu->header.frame_id = msg->child_frame_id;
sensor_msgs::ImuConstPtr imuPtr(imu);
imuCallback(imuPtr);
}
}
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;
}
