AMCL首次启动总有个烦恼的问题,就是手动初始化位姿很难给出准确的,也不会自动更新,像下面这样
像这样做成产品当然不行啦。
解决上面的问题主要有两个思想
(1)用户给个大概初始位姿,算法自动更新寻找真实值
(2)不需要用户给初始位姿,采用其它辅助方法
—————————————-分割线———————————————–
下面主要讨论第一种实现方式。
思想:让AMCL算法中在里程计没更新下自动更新粒子
1.不知道大家有没有注意AMCL算法中提供了ROS服务
nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this);
所以你只要在终端敲入
rosservice call /amcl/request_nomotion_update
粒子就会逐渐更新啦。
进一步查看AMCL中这个服务回调函数干了啥
bool AmclNode::nomotionUpdateCallback(std_srvs::Empty::Request& req,
std_srvs::Empty::Response& res)
{
m_force_update = true;
//ROS_INFO("Requesting no-motion update");
return true;
}
很明显只让一个变量为True,由此我们直接开机自动就把这个变量置True,设置个条件退出(就是这样简单了),所以直接在AmclNode::laserReceived函数中的这段后面添加
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;
需要添加的。
pf_sample_set_t *tmp = pf_->sets + pf_->current_set;
if (tmp->sample_count < (min_particles_ + 200) && m_force_update == true) //add by yuanguobin
{
//if (!autoReInitialFlag)
//{
//AutoReInitialPose(last_published_pose);
//}
//else
//{
m_force_update=false;
//}
//autoReInitialFlag = true;
}
还有一个函数需要修改void AmclNode::handleInitialPoseMessage()
applyInitialPose();
m_force_update = true; //add by yuanguobin
//autoReInitialFlag = false; //add by yuanguobin
}
以上简单修改两处,就可以试试效果
真烦CSDN不能上传视频。。
当然上面还是可能会遇到最后并没有完全找到真值,总是差一点。但是多加一个函数就可以了,假如需要使用这段代码,需要把上面修改的代码中被注释的打开。
/*add by yuanguobin */
void AmclNode::AutoReInitialPose(geometry_msgs::PoseWithCovarianceStamped & last_pose)
{
tf2::Transform pose_new;
tf2::convert(last_pose.pose.pose, pose_new);
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
pf_init_pose_cov.m[0][0] = 0.25;
pf_init_pose_cov.m[1][1] = 0.25;
pf_init_pose_cov.m[2][2] = 4.0/180.0*M_PI;
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();
pf_init_=true;
}
一个小细节提示下,假如你想给的初始较差,记得去AMCL中把pf_init_pose_cov这个值改大点。
下一期讲讲如何实现不需要人的。
欢迎各位爱好者留言和加微信讨论(15112119047)
深入AMCL(一):AMCL中轮式里程计误差模型参数
深入AMCL(二):2d激光雷达似然域测量模型