关于amcl_node.cpp，主要作用是发布一个位姿p，来表征这个粒子滤波算法对机器人当前位置的估计。这个估计的位姿，是机器人坐标系(base_link)在世界坐标系(map)下的具体坐标呈现。
另一方面呢，amcl_node.cpp也要接收外界给定的位姿来进行粒子滤波器的初始化，通俗一点讲就是将位姿初始化，初始化之后，amcl包就正式开始工作了，实时上报一个机器人位置的估计值p（位姿），供后续的导航模块作用。
1.amcl_node.cpp的功能
梳理来看， amcl_node.cpp需要做的事情有：
- 及时处理激光传感器扫描到障碍物后返回的数据data，并将这个数据data传入到观测模型，以用来更新粒子滤波器。
- 及时处理里程计的位姿data,并将这个数据data传入到运动模型，以用来更新粒子滤波器。
- 要建立坐标系之间连续转换关系,AMCL包采用ROS自带的tf2库来处理相关坐标变换关系。
- 激光传感器所在的坐标系(laser_link)到机器人所在的坐标系(base_link)是什么转换关系。（坐标系之间的转换：laser_link->base_link）
- 激光传感器扫描到的障碍物点，只会返回一个距离和角度，那么如何将这个障碍物点的坐标转换到机器人坐标系(base_link)下的坐标。（A坐标系的点要转换到B坐标系下去）
- 里程计所在的坐标系(odom)与机器人坐标系(base_link)的关系是如何定义的呢，是不是从简单的运动模型就可以推导出它俩的相关关系？(坐标系的转换:base_link->odom)
- 粒子滤波算法提供的是一个世界坐标系(map)下的机器人坐标系(base_link)的坐标。(已知A坐标系在B坐标系下的坐标)
图1.AMCL包坐标系转换示意图，摘自amcl-ROS wiki
- 从图1来看，base_link->map已经明了，base_link->odom已经明了，那么odom->map又该如何求出呢？关于base_link，odom，map相关术语的解释：
4.如何接收新的位姿以及接收新的位姿之后怎么处理。
5.怎么选出要发布的位姿呢？粒子云又是如何发布。
带着上述的问题，我们接下来对ROS自带的坐标/坐标系管理工具tf2进行深入分析，深度讨论粒子滤波器接收外界给定的位姿后该如何处理，探究AMCL包如何选出最合适的位姿发布，以及如何发布粒子云的机制。
2.管理坐标系变换关系的好工具:tf2介绍
先来个ros官方的tf2介绍铺垫铺垫：
要问怎么学会tf2，我也是看官网的Tutorials，有意思的是Tutorials的后两个比较有趣：teaches you to wait for a transform to be available on the tf2 tree when using the lookupTransform() function && teaches you about advanced time travel features of tf2.
关于tf2的相关函数，简单的可会了，比如说：
但是源码里怎么那么多关于tf2的陌生的函数？请到这里查询：
好家伙，回到AMCL包的amcl_node.cpp，看它引用的关于tf2的相关头文件：
图1.引用的tf2相关的头文件
再对比ros官网的tf2介绍截图，这里可以看到有tf2_ros,tf2_geometry_msgs可供我们查询。
图2.tf2所包含的各种类型
当然tf2_ros的相关函数的靠谱的查询，跳转至这里:
tf2_msgs又到哪里查呢？真是个问题。
tf2的更多参考链接：
这里主要声明：
图3.tf2相关术语
其它关于tf常见的广播和监听功能:
- 关于tf的广播功能：
图4.tf2的广播含义
2.关于tf的监听功能：
- lookupTransform（）函数功能：可以获得两个坐标系之间转换的关系，包括旋转和平移。
- transformPoint()函数的功能:将一个坐标系下的点的坐标转换到另一个坐标系下。
3.AMCL包的坐标系关系如何管理
这里有一篇博文讲解odom->map的坐标系转换原理比较详细，贴上来：
而在amcl_node.cpp中具体代码实现,在laserReceived函数 if(resampled || forcepublication) 条件下表达。这里贴出的图3是根据上面引用的博主博客的内容，值的注意的是AMCL主要解决的事情--估计出机器人真实的位姿(世界坐标系下)，我们在前面的AMCL系列讲的比较清晰了。
图5.AMCL包tf树实现逻辑
....
//发布位姿p，即AMCL包粒子滤波算法估计出的在世界坐标系下(map坐标系)机器人所在的位置
pose_pub_.publish(p);
last_published_pose = p;
ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);
//坐标系转换
// subtracting base to odom from map to base and send map to odom instead
// map->odom = map->base_link - base_link->odom
geometry_msgs::PoseStamped odom_to_map;
try
{
tf2::Quaternion q;
q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
//tmp_tf是base_link在global map下的坐标，即base-->map
tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
geometry_msgs::PoseStamped tmp_tf_stamped;
tmp_tf_stamped.header.frame_id = base_frame_id_;
tmp_tf_stamped.header.stamp = laser_scan->header.stamp;
//tmp_tf.inverse()是输入，tmp_tf_stamped.pose是输出
//tmp_tf_stamped是global map原点在base_link下的坐标，即map-->base
tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose);
//odom_frame_id_ default value is "odom"
//将global map原点在base_link下的坐标变换成global map原点在odom下的坐标
//即map-->odom，（相当于在odom原点看map原点的位置）
//这里的odom_to_map并非真的odom-->map，而是反过来map-->odom
this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);
}
catch(tf2::TransformException)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
}
//转换odom_to_map.pose为latest_tf_
tf2::convert(odom_to_map.pose, latest_tf_);
latest_tf_valid_ = true;
if (tf_broadcast_ == true)
{
// We want to send a transform that is good up until a
// tolerance time so that odom can be used
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
//设置tmp_tf_stamped头部信息
geometry_msgs::TransformStamped tmp_tf_stamped;
tmp_tf_stamped.header.frame_id = global_frame_id_;
tmp_tf_stamped.header.stamp = transform_expiration;
//这个变换就是child_frame_id在parent_frame_id下的坐标
tmp_tf_stamped.child_frame_id = odom_frame_id_;
//tmp_tf_stamped这个变换是odom原点在map坐标系的坐标，即odom-->map
tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);
//发布
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}
}4.AMCL包发布最合适的位姿(the only one pose)
在amcl_node.cpp中的laserReceived函数,if(resampled || forcepublication) 条件下表达：
if(resampled || force_publication)
{
// Read out the current hypotheses
double max_weight = 0.0;
int max_weight_hyp = -1;
std::vector<amcl_hyp_t> hyps;
hyps.resize(pf_->sets[pf_->current_set].cluster_count);
for(int hyp_count = 0;
hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
{
double weight;
pf_vector_t pose_mean;
pf_matrix_t pose_cov;
if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
{
ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
break;
}
//读出每个粒子sample簇（cluster）的权重，均值，协方差；
//选出其中权重最大的粒子簇；接下来将它均值和协方差初始化要发布的位姿
hyps[hyp_count].weight = weight;
hyps[hyp_count].pf_pose_mean = pose_mean;
hyps[hyp_count].pf_pose_cov = pose_cov;
if(hyps[hyp_count].weight > max_weight)
{
max_weight = hyps[hyp_count].weight;
max_weight_hyp = hyp_count;
}
}
if(max_weight > 0.0)
{
ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);
/*
puts("");
pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
puts("");
*/
geometry_msgs::PoseWithCovarianceStamped p;
// Fill in the header
p.header.frame_id = global_frame_id_;
p.header.stamp = laser_scan->header.stamp;
// Copy in the pose 利用最大权重的粒子簇cluster的均值和协方差初始化位姿p
p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
tf2::Quaternion q;
q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
tf2::convert(q, p.pose.pose.orientation);
// Copy in the covariance, converting from 3-D to 6-D
pf_sample_set_t* set = pf_->sets + pf_->current_set;
for(int i=0; i<2; i++)
{
for(int j=0; j<2; j++)
{
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
p.pose.covariance[6*i+j] = set->cov.m[i][j];
}
}
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
p.pose.covariance[6*5+5] = set->cov.m[2][2];
/*
printf("cov:\n");
for(int i=0; i<6; i++)
{
for(int j=0; j<6; j++)
printf("%6.3f ", p.covariance[6*i+j]);
puts("");
}
*/
//然后将p发布
pose_pub_.publish(p);
//并将p赋值给last_published_pose,后面有妙用
last_published_pose = p;
ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);5.AMCL包发布粒子云(particle cloud)
在amcl_node.cpp中的laserReceived函数,if(lasers_update_[laser_index])条件下表达：
// If the robot has moved, update the filter如果机器人已经动了，更新粒子滤波器
//这里主要处理激光数据
if(lasers_update_[laser_index])
{
AMCLLaserData ldata;
ldata.sensor = lasers_[laser_index];
ldata.range_count = laser_scan->ranges.size();
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
//
// Construct min and max angles of laser, in the base_link frame.
tf2::Quaternion q;
q.setRPY(0.0, 0.0, laser_scan->angle_min);
geometry_msgs::QuaternionStamped min_q, inc_q;
min_q.header.stamp = laser_scan->header.stamp;
min_q.header.frame_id = stripSlash(laser_scan->header.frame_id);
tf2::convert(q, min_q.quaternion);
q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
inc_q.header = min_q.header;
tf2::convert(q, inc_q.quaternion);
try
{
tf_->transform(min_q, min_q, base_frame_id_);
tf_->transform(inc_q, inc_q, base_frame_id_);
}
catch(tf2::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return;
}
double angle_min = tf2::getYaw(min_q.quaternion);
double angle_increment = tf2::getYaw(inc_q.quaternion) - angle_min;
// wrapping angle to [-pi .. pi]
angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;
ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);
// Apply range min/max thresholds, if the user supplied them
if(laser_max_range_ > 0.0)
ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
else
ldata.range_max = laser_scan->range_max;
double range_min;
if(laser_min_range_ > 0.0)
range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
else
range_min = laser_scan->range_min;
// The AMCLLaserData destructor will free this memory
ldata.ranges = new double[ldata.range_count][2];
ROS_ASSERT(ldata.ranges);
for(int i=0;i<ldata.range_count;i++)
{
// amcl doesn't (yet) have a concept of min range. So we'll map short
// readings to max range.
if(laser_scan->ranges[i] <= range_min)
ldata.ranges[i][0] = ldata.range_max;
else
ldata.ranges[i][0] = laser_scan->ranges[i];
// Compute bearing
ldata.ranges[i][1] = angle_min +
(i * angle_increment);
}
//使用激光数据更新粒子滤波器
lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
lasers_update_[laser_index] = false;
pf_odom_pose_ = pose;
// Resample the particles
if(!(++resample_count_ % resample_interval_))
{
pf_update_resample(pf_);
resampled = true;
}
pf_sample_set_t* set = pf_->sets + pf_->current_set;
ROS_DEBUG("Num samples: %d\n", set->sample_count);
//发布粒子云
// Publish the resulting cloud
// TODO: set maximum rate for publishing
if (!m_force_update)
{
geometry_msgs::PoseArray cloud_msg;
cloud_msg.header.stamp = ros::Time::now();
cloud_msg.header.frame_id = global_frame_id_;
cloud_msg.poses.resize(set->sample_count);
for(int i=0;i<set->sample_count;i++)
{
cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
cloud_msg.poses[i].position.z = 0;
tf2::Quaternion q;
q.setRPY(0, 0, set->samples[i].pose.v[2]);
tf2::convert(q, cloud_msg.poses[i].orientation);
}
particlecloud_pub_.publish(cloud_msg);
}
}6.粒子滤波器接收外界给定的位姿后的处理
处理初始位姿的函数：handleInitialPoseMessage()
void
AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& 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(stripSlash(msg.header.frame_id) != global_frame_id_)
{
ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",
stripSlash(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.
geometry_msgs::TransformStamped tx_odom;
try
{
ros::Time now = ros::Time::now();
// wait a little for the latest tf to become available
tx_odom = tf_->lookupTransform(base_frame_id_, msg.header.stamp,
base_frame_id_, ros::Time::now(),
odom_frame_id_, ros::Duration(0.5));
}
catch(tf2::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());
tf2::convert(tf2::Transform::getIdentity(), tx_odom.transform);
}
tf2::Transform tx_odom_tf2;
tf2::convert(tx_odom.transform, tx_odom_tf2);
tf2::Transform pose_old, pose_new;
tf2::convert(msg.pose.pose, pose_old);
pose_new = pose_old * tx_odom_tf2;
// 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(),
tf2::getYaw(pose_new.getRotation()));
// 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] = tf2::getYaw(pose_new.getRotation());
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();
}其中提到的applyInitialPose(),用于设置位姿：
void
AmclNode::applyInitialPose()
{
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
if( initial_pose_hyp_ != NULL && map_ != NULL ) {
pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov);
pf_init_ = false;
delete initial_pose_hyp_;
initial_pose_hyp_ = NULL;
}
}7.核心函数:laserReceived()的关键条件分支
下面贴的代码片是laserReceived()函数的关键条件分支，下一讲我们再对laserReceived函数进行分析。
图6.laserReceived函数关键条件分支展示