摘自：https://blog.csdn.net/dieju8330/article/details/96770964

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

dieju8330 2019-07-24 21:56:08 2896 收藏 41

分类专栏： 2D激光SLAM ROS

版权

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();
}