1.主函数
主函数主要作用是：
-
定义一个信号变量，管理节点
- 定义amclNode对象
int
main(int argc, char** argv)
{
ros::init(argc, argv, "amcl");
ros::NodeHandle nh;
// Override default sigint handler
signal(SIGINT, sigintHandler);
// Make our node available to sigintHandler
amcl_node_ptr.reset(new AmclNode());
if (argc == 1)
{
// run using ROS input
ros::spin();
}
else if ((argc == 3) && (std::string(argv[1]) == "--run-from-bag"))
{
amcl_node_ptr->runFromBag(argv[2]);
}
// Without this, our boost locks are not shut down nicely
amcl_node_ptr.reset();
// To quote Morgan, Hooray!
return(0);
}
2.amclNode对象的构造
1.有一个配置相关的递归互斥锁锁
2.读取和配置参数; 只写一个说明
if (private_nh_.hasParam("beam_skip_error_threshold_"))
{
private_nh_.param("beam_skip_error_threshold_", beam_skip_error_threshold_);
}
else
{
private_nh_.param("beam_skip_error_threshold", beam_skip_error_threshold_, 0.9);
}3.从服务中跟新位姿 updatePoseFromServer(); 该函数就是将参数从参数列表中读取出
代码中也是从param中读取 包括初始位姿+协方差
init_pose_[0] = 0.0;
init_pose_[1] = 0.0;
init_pose_[2] = 0.0;
init_cov_[0] = 0.5 * 0.5;
init_cov_[1] = 0.5 * 0.5;
init_cov_[2] = (M_PI/12.0) * (M_PI/12.0);4.定义接听和发送tf
cloud_pub_interval.fromSec(1.0);
tfb_ = new tf::TransformBroadcaster();
tf_ = new TransformListenerWrapper();5.发布话题和服务
pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);
global_loc_srv_ = nh_.advertiseService("global_localization",
&AmclNode::globalLocalizationCallback,
this);
nomotion_update_srv_= nh_.advertiseService("request_nomotion_update",
&AmclNode::nomotionUpdateCallback, this);
set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this);6.订阅激光话题和数据
可以订阅任何ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（一般在回调函数处理）。
laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
laser_scan_filter_ =
new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,*tf_,odom_frame_id_,100);
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,this, _1));7.订阅初始位姿（位姿+协方差+time）
initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
8.是否使用话题地图，否则使用服务地图
if(use_map_topic_) {
map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);
ROS_INFO("Subscribed to map topic.");
} else {
requestMap();
}9.使用动态设置参数和创建激光检测时间
dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~"));
dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
// 15s timer to warn on lack of receipt of laser scans, #5209
laser_check_interval_ = ros::Duration(15.0);
check_laser_timer_ = nh_.createTimer(laser_check_interval_,
boost::bind(&AmclNode::checkLaserReceived, this, _1));
3.激光回调函数
laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
1.地图为空时直接返回，并且锁住一些配置参数
last_laser_received_ts_ = ros::Time::now();
if( map_ == NULL ) {
return;
}
boost::recursive_mutex::scoped_lock lr(configuration_mutex_);2.支持多个激光的使用
利用frame_id判断，将新的激光添加到激光这个容器中
1. 当激光数据的franme_id不一样
> 1.跟新信息 -- 激光数据、状态量
ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(),
laser_scan->header.frame_id.c_str());
lasers_.push_back(new AMCLLaser(*laser_));
lasers_update_.push_back(true);
laser_index = frame_to_laser_.size();
> 2.接听 scan_link->base_link变化
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
tf::Vector3(0,0,0)),
ros::Time(), laser_scan->header.frame_id);
tf::Stamped<tf::Pose> laser_pose;
try
{
this->tf_->transformPose(base_frame_id_, ident, laser_pose);
}
catch(tf::TransformException& e)
{
ROS_ERROR("Couldn't transform from %s to %s, "
"even though the message notifier is in use",
laser_scan->header.frame_id.c_str(),
base_frame_id_.c_str());
return;
}
> 3.记录该frame_id激光 相对于base_link的位姿
pf_vector_t laser_pose_v;
laser_pose_v.v[0] = laser_pose.getOrigin().x();
laser_pose_v.v[1] = laser_pose.getOrigin().y();
// laser mounting angle gets computed later -> set to 0 here!
laser_pose_v.v[2] = 0;
lasers_[laser_index]->SetLaserPose(laser_pose_v);
ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
laser_pose_v.v[0],
laser_pose_v.v[1],
laser_pose_v.v[2]);
frame_to_laser_[laser_scan->header.frame_id] = laser_index;
}2. 当激光数据的franme_id一样
// we have the laser pose, retrieve laser index
laser_index = frame_to_laser_[laser_scan->header.frame_id]; //有了就直接调用序号
3.得到里程计的位姿 getOdomPose
bool
AmclNode::getOdomPose(tf::Stamped<tf::Pose>& odom_pose,
double& x, double& y, double& yaw,
const ros::Time& t, const std::string& f)
{
// Get the robot's pose
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
tf::Vector3(0,0,0)), t, f);
try
{
this->tf_->transformPose(odom_frame_id_, ident, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
x = odom_pose.getOrigin().x();
y = odom_pose.getOrigin().y();
double pitch,roll;
odom_pose.getBasis().getEulerYPR(yaw, pitch, roll);
return true;
}4.只有里程计满足一定条件才跟新 粒子滤波
amcl不是激光来一帧就进行运动型一次，而是满足一定的位移情况时，才设置激光已经跟新，才进行后面的粒子匹配，首先得到里程计位姿，然后判断前后两次的位姿变化
pf_vector_t delta = pf_vector_zero(); //粒子分布均值=分布函数pf_init_ 指：位姿是否已经初始化过，一般在重新设置初始位姿时，其会设置成false
4.1. 当pf_init_位姿已经初始化后，根据里程计是否满足一定条件确定过是否跟新激光数据
if(pf_init_) //不是第一桢，即存在上一桢，计算里程计运动是否达到粒子更新阈值
{
// Compute change in pose
//delta = pf_vector_coord_sub(pose, pf_odom_pose_);
delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
// See if we should update the filter
bool update = fabs(delta.v[0]) > d_thresh_ ||
fabs(delta.v[1]) > d_thresh_ ||
fabs(delta.v[2]) > a_thresh_;
update = update || m_force_update; //更新或者强制更新
m_force_update=false;
// Set the laser update flags
if(update)
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true; //senser update（多个激光雷达）
}4.2. 当pf_init_位姿未初始化，最后一次过滤更新，激光数据得到跟新，强制发布标志记为true
if(!pf_init_) //第一桢，不需要里程计更新
{
// Pose at last filter update
pf_odom_pose_ = pose;
// Filter is now initialized
pf_init_ = true;
// Should update sensor data
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true; //----
force_publication = true;
resample_count_ = 0;
}4.3. 不是第一桢（粒子滤波），而且达到运动阈值。更新受到的这个激光雷达的
// If the robot has moved, update the filter
else if(pf_init_ && lasers_update_[laser_index])
//不是第一桢（粒子滤波），而且达到运动阈值。更新受到的这个激光雷达的
{
//printf("pose\n");
//pf_vector_fprintf(pose, stdout, "%.3f");
AMCLOdomData odata;
odata.pose = pose; //base/odom
// HACK
// Modify the delta in the action data so the filter gets
// updated correctly
odata.delta = delta; //now/-上一桢粒子
// Use the action data to update the filter
odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
//运动更新！！！ 把所有粒子在里程计测量点附近高斯撒点
// Pose at last filter update
//this->pf_odom_pose = pose;
}
5.如果激光数据已经跟新时 //处理激光数据
5.1.检测点云数据，防止其颠倒
在base_link框架中构建激光的最小和最大角度，并将其放到[-pi .. pi]
为了解释颠倒安装的激光，我们确定了激光器在基架中的最小，最大和增量角。
tf::Quaternion q; //激光雷达最小角向量
q.setRPY(0.0, 0.0, laser_scan->angle_min); //roll，。。，Yaw
tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment); // min+一个分辨率
tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
try
{
tf_->transformQuaternion(base_frame_id_, min_q, min_q); //转在base坐标系下
tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return;
}
double angle_min = tf::getYaw(min_q);
double angle_increment = tf::getYaw(inc_q) - angle_min;
// wrapping angle to [-pi .. pi]
angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI; //计算x/y的余数 ？？？
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; //小于最小值，则赋值最大？因为likelihood——filed中会直接丢弃最大值
else
ldata.ranges[i][0] = laser_scan->ranges[i]; //距离
// Compute bearing
ldata.ranges[i][1] = angle_min +
(i * angle_increment); //角度
}5.2计算所有粒子跟新权重，并归一化
//传感器测量更新权重，UpdateSensor在amcl_laser。cpp中
lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
//pf_update_sensor实现对所有粒子更新权重，并归一化、
//计算出《概率机器人》8.3.5的失效恢复的长期似然和短期似然
lasers_update_[laser_index] = false;
5.3检查重采样条件，以及重采样
按照一定的规则重采样粒子，包括前面说的失效恢复、粒子权重等，然后放入到kdtree，暂时先理解成关于位姿的二叉树，然后进行聚类，得到均值和方差等信息,就是将相近的一堆粒子融合成一个粒子
// Resample the particles
if(!(++resample_count_ % resample_interval_))
{
pf_update_resample(pf_);
resampled = true;
}
//重采样后粒子集，pf—》set指向数组起始位置，current=0或1，输出的就是set中0，1交替
pf_sample_set_t* set = pf_->sets + pf_->current_set;
ROS_DEBUG("Num samples: %d\n", set->sample_count); 5.4发布点云结果
// 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++)
{
tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
tf::Vector3(set->samples[i].pose.v[0],
set->samples[i].pose.v[1], 0)),
cloud_msg.poses[i]);
}
particlecloud_pub_.publish(cloud_msg); //新粒子发布到全局坐标系下
} 6.得到最后位姿，并发布
遍历所有粒子簇，找出权重均值最大的簇，其平均位姿就是我们要求的机器人后验位姿，到此一次循环已经所有完成
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;
}
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
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];
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
p.pose.pose.orientation);
// Copy in the covariance, converting from 3-D to 6-D ，why，为什么转换为6维的
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("");
}
*/
pose_pub_.publish(p);
last_published_pose = p; 发布map->odom
`` c ++
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
// listen base to odom
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
}
catch(tf::TransformException)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
}
latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
tf::Point(odom_to_map.getOrigin()));
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_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}
}
else
{
ROS_ERROR("No pose!");
}
7.如果激光不跟新时 接听map->odom 然后发布
if (tf_broadcast_ == true)
{
// Nothing changed, so we'll just republish the last transform, to keep
// everybody happy.
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
}
// Is it time to save our last pose to the param server
ros::Time now = ros::Time::now();
if((save_pose_period.toSec() > 0.0) &&
(now - save_pose_last_time) >= save_pose_period)
{
this->savePoseToServer();
save_pose_last_time = now;
}
}
来源：oschina
链接：https://my.oschina.net/u/4329790/blog/4467693