2D激光SLAM：：AMCL发布的odom----map座标TF变换解读

AMCL发布的odom----map座标TF变换解读

---

一、背景

1、AMCL的作用是什么？

答：AMCL是基于蒙特卡洛定位方法的一种定位方法实现，集成于ROS操作系统的navigation导航包里面，简单来说，AMCL就是得到一个相对准确的机器人的世界座标。

2、为什么需要AMCL？机器人地盘不是都有里程计和陀螺仪吗？

答：

（1）里程计会有累计误差，即跑远了之后，这个误差会一直叠加，另外，还会出现车轮打滑的现象，这时候的里程计信息就不准确了。

（2）陀螺仪是基于积分型的，也会出现误差。

3、AMCL获取到机器人的世界座标之后，如何把这个信息传达给navigation导航包的move_base路径规划模块？ 

答：这个就涉及到本文要说的，amcl获取到机器人的世界座标之后，经过一系列变换，最后发布一个tf变换，关于odom-----map两个座标系的TF变换

---

 二、关于odom和map座标系

1、odom

odom指里程计的座标系，这个座标系在小车启动底盘的时候建立（即以小车的起点为原点），以小车前进方向为X轴（在ROS中，座标系是右手，拇指、食指和中指），张开这三个手指，相互垂直，然后食指指向正前方，此时，中指的方向就是Y轴，拇指则是Z轴。

 2、map

map我个人理解为全局地图也就是全局座标系---global map

global map同样也是在机器人启动的时候建立，座标系的方向跟odom一致。

到这里就会有疑惑，odom和map两个座标系不应该就重合了吗？

答：在机器人刚启动的时候，这两个座标系的确是重合的，但是跑远了之后，由于里程计的误差，会使得odom座标系与global map座标系的原点不重合，（个人理解是，按照里程计给出的机器人当前位姿，然后根据这个数据返回，最后机器人返回的地方并非一开始启动时的原点）

刚启动时，

跑远之后，或者车轮打滑了【可以看到，map没有动，这是机器人真正的起点，而odom座标系的原点已经偏移了，就是说，依照里程计给出的机器人位姿信息，逆着反推回去，只能回到下图红色的odom‘，而不是最初的起点】

---

 三、如何修正？

发布一个odom----map的TF座标变换即可

 1、如何发布？

首先需要明确一些必须的信息，要发布这个TF座标变换，需要得到一个重要的参数---【机器人真实的位姿（世界座标系下）】但是如何获取，这是AMCL主要解决的事情，这里不过多的关注

1）假定我们得到了base_link在世界座标系global_map的座标变换tmp_tf （即base_link在global_map下的座标）

2）那么tmp_tf.inverse()则表示世界座标系global_map到base_link的座标变换（即global_map在base_link下的座标）

3）由于base_link到odom座标系的座标变换是可以直接知道的（即base_link在odom下的座标）

4）因此，使用tf.transformPose可以获得global_map到odom的变换tmp1_tf，（即global_map原点在odom座标系下的座标)

5）最后，对tmp1_tf求逆，得到odom-->map的变换，（即odom在global_map座标系下的座标）

6）发布odom-->map即可实现修正

四、AMCL模块关于此TF变换的源码（带注释，可结合上述过程来看）

      //发布最大权重的集群的pose统计值
      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;
      }
    }

---

五、个人仿照AMCL发布的TF变换源码（使用tf包替代了AMCL使用的tf2，感觉tf2比较小众？）

为了方便测试，代码中的真实位姿使用了odom的信息来替代了

#include <ros/ros.h>
#include <iostream>
#include <nav_msgs/Odometry.h>
#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "message_filters/subscriber.h"
#include "tf/message_filter.h"
#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"
#include "nav_msgs/OccupancyGrid.h"
//#include "tf2/utils.h"
#include "math.h"
//#include "tf2/convert.h"
//#include "tf2_ros/message_filter.h"
using namespace std;

tf::TransformListener *tf_;
tf::TransformBroadcaster *tfb_;
nav_msgs::Odometry::ConstPtr oppp;

// The basic vector
typedef struct
{
  double v[3]={0};
} pose_vector_t;

static double normalize(double z)
{
  return atan2(sin(z),cos(z));
}
static double angle_diff(double a, double b)
{
  double d1, d2;
  a = normalize(a);
  b = normalize(b);
  d1 = a-b;
  d2 = 2*M_PI - fabs(d1);
  if(d1 > 0)
    d2 *= -1.0;
  if(fabs(d1) < fabs(d2))
    return(d1);
  else
    return(d2);
}

// Return a zero vector
pose_vector_t pose_vector_zero()
{
  pose_vector_t c;

  c.v[0] = 0.0;
  c.v[1] = 0.0;
  c.v[2] = 0.0;

  return c;
}

void pose_vector_setValue(pose_vector_t * c,double x,double y,double yaw)
{
    double *v;
    v=c->v;
    *v=x;
    v++;
    *v=y;
    v++;
    *v=yaw;
    //c->(v+1)=y;
    //*c->(v=yaw;
}

pose_vector_t lastPose_v;
tf::Transform lastTransfrom_map_in_odom;

void odomMsgCallback(const nav_msgs::Odometry::ConstPtr &odomMsg){

    static bool init=false;
    static double delta_x=0;
    static bool forward=true;
    ros::Duration transform_tolerance_;
    transform_tolerance_.fromSec(0.1);
    //位姿偏移量初始化
    pose_vector_t delta = pose_vector_zero();
    lastTransfrom_map_in_odom=tf::Transform(tf::createQuaternionFromRPY(0, 0, 0),
            tf::Vector3(0, 0, 0));

    if(!init){
        init=true;
        pose_vector_setValue(&lastPose_v,
                             odomMsg->pose.pose.position.x,
                             odomMsg->pose.pose.position.y,
                             tf::getYaw(odomMsg->pose.pose.orientation));

    }else{
        delta.v[0] = odomMsg->pose.pose.position.x - lastPose_v.v[0];
        delta.v[1] = odomMsg->pose.pose.position.y - lastPose_v.v[1];
        delta.v[2] = angle_diff(tf::getYaw(odomMsg->pose.pose.orientation), lastPose_v.v[2]);

        //判断位移偏移量是否大于阈值
        if(true/*sqrt(pow(delta.v[0],2)+pow(delta.v[1],2))>=0*/){

            /******************发布座标变换**********************************/
            //取odom获取的位姿作为真实位姿
            pose_vector_t truePose_v;
            truePose_v.v[0]=odomMsg->pose.pose.position.x+delta_x;
            truePose_v.v[1]=odomMsg->pose.pose.position.y;
            truePose_v.v[2]=tf::getYaw(odomMsg->pose.pose.orientation);

            tf::Stamped<tf::Transform> map_in_odom;
            tf::Stamped<tf::Transform> odom_in_map;
            //tf::Transform map_in_odom;
            try{
                //创建一个基于global map的座标
                tf::Stamped<tf::Pose> ident(tf::Transform(tf::createQuaternionFromRPY(0, 0, truePose_v.v[2]),
                                                          tf::Vector3(truePose_v.v[0], truePose_v.v[1], 0)),odomMsg->header.stamp, "map");
                //创建上面座标的逆，即global map原点在Base_link座标系下的座标
                tf::Stamped<tf::Pose> tmp(ident.inverse(),odomMsg->header.stamp, "base_link");
                //然后将该座标转换到odom座标系下
                //得到的map_in_odom是 global map原点在odom座标系下的座标
                tf_->transformPose("odom", tmp, map_in_odom);

                //求逆变换
                //得到的odom_in_map是 odom座标原点在global map座标系下的座标
                //即得到从odom座标系到global map座标系的变换矩阵
                odom_in_map.setData(map_in_odom.inverse());
                odom_in_map.frame_id_="map";
                odom_in_map.stamp_=odomMsg->header.stamp+transform_tolerance_;

                ROS_INFO("calculate odom in map success");
                ROS_INFO("odom in map x:[%f] y:[%f] yaw:[%f]",
                         odom_in_map.getOrigin().x(),
                         odom_in_map.getOrigin().y(),
                         odom_in_map.getRotation().getAngle());

                ROS_INFO("now send the TF Broadcast odom_in_map");

                tfb_->sendTransform(tf::StampedTransform(odom_in_map,odom_in_map.stamp_,"map","odom"));

                lastTransfrom_map_in_odom=odom_in_map;

            }catch(tf::TransformException e){
                ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
                return ;
            }
//            ROS_INFO("now send the TF Broadcast odom_in_map");
//            odom_in_map.setData(tf::Transform(tf::createQuaternionFromRPY(0, 0, 0),
//                                                          tf::Vector3(0, 0, 0)));
//            odom_in_map.frame_id_="map";
//            odom_in_map.stamp_=odomMsg->header.stamp+transform_tolerance_;
//            tfb_->sendTransform(tf::StampedTransform(odom_in_map,odom_in_map.stamp_,"map","odom"));
            if(delta_x<=0){
                forward=true;
            }
            if(delta_x>=10){
                forward=false;
            }
            if(forward)
                delta_x+=0.02;
            else
                delta_x-=0.02;

//            pose_vector_setValue(&lastPose_v,
//                                 odomMsg->pose.pose.position.x,
//                                 odomMsg->pose.pose.position.y,
//                                 tf::getYaw(odomMsg->pose.pose.orientation));
        }else{
            //位移偏移量没有达到阈值
            //发布之前的变换
            ROS_INFO("now send the TF Broadcast odom_in_map");
            tfb_->sendTransform(tf::StampedTransform(lastTransfrom_map_in_odom,odomMsg->header.stamp+transform_tolerance_,"map","odom"));
        }
    }

//    tf::Stamped<tf::Transform> odom_in_map;
//    tfb_->sendTransform(tf::StampedTransform(odom_in_map,odom_in_map.stamp_,"map","odom"));

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "publish_tf_node");
    ros::NodeHandle nh;

    tf_=new tf::TransformListener;
    tfb_=new tf::TransformBroadcaster;
    message_filters::Subscriber<nav_msgs::Odometry>* odom_sub_;
    //message_filters::Subscriber<sensor_msgs::LaserScan>* scan_sub_;
    tf::MessageFilter<nav_msgs::Odometry>* odom_filter_;

    // Subscribers
    //订阅"odom"
    ROS_INFO("subscribed the topic \"odom\" ");
    odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh, "odom", 100);
    //odom_sub_->registerCallback(odomMsgCallback/*boost::bind(odomMsgCallback, this, _1)*/);
    odom_filter_ =new tf::MessageFilter<nav_msgs::Odometry>(*odom_sub_, *tf_, "base_link", 100);
    odom_filter_->registerCallback(odomMsgCallback);
    ros::spin();

}