amcl定位过程中发布机器人在地图中位姿的总结

AMCL(adaptive Monte Carlo Localization)自适应蒙特卡洛定位，A也可以理解为augmented，是机器人在二维移动过程中概率定位系统，采用粒子滤波器来跟踪已经知道的地图中机器人位姿，对于大范围的局部定位问题工作良好。对机器人的定位是非常重要的，因为若无法正确定位机器人当前位置，那么基于错误的起始点来进行后面规划的到达目的地的路径必定也是错误的.

目前要实现的应用是amcl匹配后，得到车体位姿，然后发布到外部使用

起初我是采用订阅amcl发布的amcl_pose话题的方法

代码如下

参考ROS：订阅话题并发布（订阅amcl_pose数据并发布）_Better-ing的博客-CSDN博客

class amcl_pose_sub_pub {
private:
    ros::NodeHandle nh_;  // 定义ROS句柄
    ros::Publisher msgPointPub; 
    ros::Subscriber sub;
 
public:
    amcl_pose_sub_pub()
    {
    msgPointPub = nh_.advertise<geometry_msgs::PointStamped>("amcl_pose_", 1000);
    sub = nh_.subscribe("/amcl_pose", 1, &amcl_pose_sub_pub::amcl_pose_CallBack,this);
    }
 
    ~amcl_pose_sub_pub(){}
 
    void amcl_pose_CallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
    {
        geometry_msgs::PointStamped msgPointStamped;
        msgPointStamped.point=msg->pose.pose.position;
        msgPointStamped.header.stamp = msg->header.stamp;
        msgPointStamped.header.frame_id = "map";
        msgPointPub.publish(msgPointStamped);
        std::cout<<"amcl"<<std::endl;
    }
};
int main(int argc, char** argv)
{
    ros::init(argc, argv,"amcl_pose_sub_pub");
    amcl_pose_sub_pub obj;
    ros::spin();
    return 0;
}

效果如图 （rviz里面添加pointstanted订阅发布的amcl_pose话题，图中紫色的点）

但是发现amcl_pose的频率很低。amcl_pose话题的发布需要经过重采样，而重采样的触发条件是里程计显示小车平移了0.2m(参数update_min_d)或者旋转了30度（参数update_min_a）。而且我在rviz里点击了重新设置车体坐标的按钮之后才能显示出来

但是这两个参数阈值如果改的很小，虽然amcl_pose可以很快发布的，但是这就会导致例子很快收敛而不能定位正确。所以直接获取amcl_pose的方法是不可行的。

参考之前使用cartographer纯定位的时候，通过tf树获取车体位姿的办法

// //----------------------------------------------------------
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
#include <iostream>
#include <cmath>
using namespace std;
double x,y,z,ww,zz,hh,ii,Aww,Azz,Ahh,Aii;
double theta;
int main(int argc, char** argv){
  ros::init(argc, argv, "tf_subscriber");
 
  ros::NodeHandle node;
  tf::TransformListener listener;
  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
        //得到坐标odom和坐标base_link之间的关系
      listener.waitForTransform("map","base_footprint",  ros::Time(0), ros::Duration(3.0));
      listener.lookupTransform("map", "base_footprint", ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
 
    x=transform.getOrigin().x();
    y=transform.getOrigin().y();
    z=transform.getOrigin().z();
  
    tf::Quaternion q = transform.getRotation();
    double qx = q.x();
    double qy = q.y();
    double qz = q.z();
    double qw = q.w();
 
    printf("x: %f, y: %f, z: %f, qx: %f,qy: %f,qz: %f, qw: %f\n",x,y,z,qx,qy,qz,qw);
 
    rate.sleep();
  }
  return 0;
};

效果如下