ROS -- 最简单的自主ACML定位

关于定位的原理和算法：

ARUANTOU：自适应MCL（KLD采样）
ARUANTOU：定位算法 -- MCL蒙特卡洛（粒子）滤波

在Ros实现中，有一个AMCL的package，用的就是自适应滤波算法。这里主要记录下自主定位在ros中的实现方式。

我们知道在粒子滤波定位的时候，需要结合地图数据和机器人的初始位姿通过不断的移动之后们，机器人就可以知道自己所在的位置。

在ros中我们可以通过这个button来进行初始位姿，但是这还是一个人为的过程，在实际应用中还是需要自动化的，这样会显得更大气上档次。

AMCL包是很强大的，研究这个包居然发现自身是支持自动自主定位的。内部实现为一个service -- global_location. 我们只需要每次在运行amcl之后，通过client调用一下这个service，它就会在整个地图数据的free空间布满粒子，这样机器人就可以通过自己的移动来不断的过滤筛选出概率最高的那些粒子最终实现定位的功能。

先看下client的实现：

#include "ros/ros.h"
#include "std_srvs/Empty.h"
#include <unistd.h>
int main(int argc, char** argv)
{
    ros::init(argc, argv, "DeliverRobot");
    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("global_localization");
    std_srvs::Empty srv;

    int trytimes = 0;
    while(!(client.call(srv)) && trytimes < 5)
    {
        ROS_ERROR("Failed call global localization");
        trytimes++;
        sleep(1);
        continue;
    }
    if(trytimes == 5)
    {
        ROS_ERROR("Has try 5 times No useful!!!!!!!");
    }
    else
    {
        ROS_INFO("call global localization successful");
    }
    return 0;
}

然后通过修改launch文件，增加client node的添加：

<node pkg="deliver" type="deliver" name="deliver" output="screen"></node>

只要加上这一句，在amcl启动后，client会请求这个service，然后我们就可以通过rviz中看到最终的定位情况：