void
AmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
boost::recursive_mutex::scoped_lock prl(configuration_mutex_);
if(msg->header.frame_id == "")
{
// This should be removed at some point
ROS_WARN("Received initial pose with empty frame_id. You should always supply a frame_id.");
}
// We only accept initial pose estimates in the global frame, #5148.
else if(tf_->resolve(msg->header.frame_id) != tf_->resolve(global_frame_id_))
{
ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",
msg->header.frame_id.c_str(),
global_frame_id_.c_str());
return;
}
// In case the client sent us a pose estimate in the past, integrate the
// intervening odometric change.
tf::StampedTransform tx_odom;
try
{
tf_->lookupTransform(base_frame_id_, ros::Time::now(),
base_frame_id_, msg->header.stamp,
global_frame_id_, tx_odom);
}
catch(tf::TransformException e)
{
// If we've never sent a transform, then this is normal, because the
// global_frame_id_ frame doesn't exist. We only care about in-time
// transformation for on-the-move pose-setting, so ignoring this
// startup condition doesn't really cost us anything.
if(sent_first_transform_)
ROS_WARN("Failed to transform initial pose in time (%s)", e.what());
tx_odom.setIdentity();
}
tf::Pose pose_old, pose_new;
tf::poseMsgToTF(msg->pose.pose, pose_old);
pose_new = tx_odom.inverse() * pose_old;
// Transform into the global frame
ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",
ros::Time::now().toSec(),
pose_new.getOrigin().x(),
pose_new.getOrigin().y(),
getYaw(pose_new));
// Re-initialize the filter
pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
pf_init_pose_mean.v[2] = getYaw(pose_new);
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
// Copy in the covariance, converting from 6-D to 3-D
for(int i=0; i<2; i++)
{
for(int j=0; j<2; j++)
{
pf_init_pose_cov.m[i][j] = msg->pose.covariance[6*i+j];
}
}
pf_init_pose_cov.m[2][2] = msg->pose.covariance[6*5+5];
delete initial_pose_hyp_;
initial_pose_hyp_ = new amcl_hyp_t();
initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
applyInitialPose();
}
