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

}

2D激光SLAM：：AMCL發佈的odom----map座標TF變換解讀

AMCL發佈的odom----map座標TF變換解讀

C/C++：：淺談.obj .exe .dll .lib關係&& .o .a .so關係

樹莓派學習：：qt5.10.1交叉編譯【帶opengl ES2】到非官方64位系統

ROS：：ROS_INFO帶顏色的輸出

SLAM：：g2o安裝和使用

聯想 Yoga C740：：關於Ubuntu16.04下無法識別Intel WIFI6 AX201無線網卡的解決方案

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結