在用機器人和鐳射雷達跑SLAM演算法構建地圖時，總感覺鐳射雷達資料在RVIZ中顯示的方向有問題（前提是鐳射雷達的安裝方向正確），知道是tf座標轉換存在問題，但一直沒有調整到好的狀態，所以查了些相關資料，終於搞明白了ROS中的座標轉換是怎麼回事，於是記錄下來。

一、首先要搞明白URDF、TF和odom的關係

ROS 中對於多座標系的處理是使用樹型表示，在機器人自主導航中，ROS會構建這幾個很重要的座標系。一般在urdf檔案中都要定義base_link，它代表了機器人的主幹，其它所有的frame都是相對於base_link定義並粘在一起的。它們一起相對於大地圖map移動，讓機器人移動就是向tf釋出 geometry_msgs::TransformStamped 訊息通知ros  base_link相對於map的tf轉換關係。先看一下這幾個概念在ros中的定義：

base_link: 一般位於tf tree的最根部，物理語義原點一般為表示機器人中心，為相對機器人的本體的座標系。

odom：一般直接與base_link 相連結，語義為一個對於機器人全域性位姿的粗略估計。取名來源於odometry（里程計），一般這個座標系的資料也是來源於里程計。對於全域性位姿的估計方法很多，比如在hector SLAM與導航體系中，就採用了imu資料估計全域性位姿，還有很多視覺里程計的演算法（visual odometry）也能提供位姿估計。原點為開始計算位姿那個時刻的機器人的位置。

map: 一般與odom（或者odom_combined）相連，語義為一個經過先驗（或者SLAM）地圖資料矯正過的，在地圖中的位姿資訊。與odom同為全域性座標系。原點為地圖原點（地圖原點在地圖相應的yaml檔案中有規定）。

各個Frames的關係

frame之間是按樹狀結構組織的。所以每個frame只有一個父節點和任意多個子節點。 上述幾個frame的關係:

map --> odom --> base_link

odom到base_link的座標轉換是從運動源計算出來廣播的。map到base_link的座標轉換是被定位模組計算出來的. 但定位模組不釋出map到base_link的轉換. 相反它先接受從odom到base_link的轉換, 再計算並廣播map到odom的位置轉換關係。

fixed_frame：RViz中認定的大世界就是fixed_frame

target_frame：Rviz中視覺跟蹤的frame是 target_frame

二、座標轉換的兩種方法

方法一：修改啟動檔案

在lidar的launch啟動檔案中增加：

<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser4" 
    args="0.0 0.0 0.2 0.0 3.1415926 0.0 /base_link /laser_frame 40" />
</launch>

其中args="0.0 0.0 0.2 0.0 3.1415926 0.0 /base_link /laser_frame 40"引數依次對應的含義是：static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms，其中 x y z 是 x y z 的座標， yaw pitch roll 是 繞 z旋轉弧度角，繞y旋轉弧度角，繞x旋轉弧度角，frame_id child_frame_id period_in_ms 父座標系，子座標系，釋出間隔（ms），上述是指的 z方向增加0.2m，繞y旋轉180°。（其中注意 yaw pitch roll 不要弄錯，可以通過rviz檢視， 紅色是x ， 綠色是y， 藍色是z）

方法二：修改urdf檔案的座標

找到urdf的描述檔案，如turtlebot_description/urdf/turtlebot_library.urdf.xacro，在檔案中修改座標

<joint name="laser" type="fixed">
    <origin xyz="0.00 0.00 0.20" rpy="0 3.1415926 0" />
    <parent link="base_link" />
    <child link="base_laser_link" />
  </joint>

  <link name="base_laser_link">
    <visual>
      <geometry>
        <box size="0.00 0.05 0.06" />
      </geometry>
      <material name="Green" />
    </visual>
    <inertial>
      <mass value="0.000001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
        iyy="0.0001" iyz="0.0"
        izz="0.0001" />
    </inertial>
  </link>

三、座標的校正方法

本節以校正F4鐳射雷達與D1機器人的座標為例，其它機器人校正方法類似，F4 與 D1 的座標校對，是通過對launch檔案中的引數進行修改來使兩者座標一致的，要執行哪個launch檔案就要對哪個launch檔案中的引數進行修改，使兩者座標一致的。在本教程中，便是要修改 gmapping_demo.launch 檔案，找到並開啟該檔案

<launch>
<node name="arduino" pkg="dashgo_bringup" type="dashgo_driver.py" output="screen">
<rosparam file="$(find dashgo_bringup)/config/my_dashgo_params.yaml" command="load" />
</node>

<node name="flashgo" pkg="flashgo" type="flashgo_node" output="screen">
<param name="serial_port" type="string" value="/dev/flashlidar"/>
<param name="serial_baudrate" type="int" value="230400"/>
<param name="frame_id" type="string" value="laser_frame"/>
<param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/>
</node>

<include file="$(find dashgo_description)/launch/dashgo_description.launch"/>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser4"
args="0.0 0.0 0.2 0.0 3.1415926 0.0 /base_link /laser_frame 40" /> //修改args的引數值

<include file="$(find dashgo_nav)/launch/gmapping.launch"/>
<include file="$(find dashgo_nav)/launch/move_base.launch"/>
</launch>

args="0.0 0.0 0.2 0.0 3.1415926 0.0 /base_link /laser_frame 40"，在 /base_link 前面的 6 個引數，只需要調整前面5個引數，最後一個預設為 0.0 即可，這 5 個引數都是相對於 D1 的座標系來調整的。前面 3 個，分別表示鐳射雷達 F4 在X、Y、Z軸（右手定則）上距離（0 , 0 , 0）點的座標位置,（0 , 0 , 0）點是 D1 的座標系原點，該點是 D1 的重心點。後面 2 個，分別表示沿著 D1 中心線（正前方與正後方連成的直線）左右方向、上下方向移動偏離的角度，大小範圍為 -3.1415926 ~ 3.1415926 ，-3.1415926 為-180度， 3.1415926 為180度。

前後方向校正，只需修改第 4 個引數，D1 的前方是有三個超聲波模組的，中間的超聲波模組就是D1的正前方，D1 的後方是單獨一個超聲波模組的，也是D1的正後方，D1 的前後方向瞭解之後，便調整 F4 的座標方向與 D1 的一致即可。

一般來說，F4 已經固定在 D1 已設定的位置上，而且 F4 在不停旋轉，便要借用較大塊、較平整的擋板，如：大紙箱，來確定 F4 當前的方位。先將擋板橫擺在 D1 正前方，擋板中心與 D1 正前方那個超聲波模組成一條直線，擋板的面要與直線成90度角。然後，對照rviz中 F4 的掃描顯示，D1 正前方是否橫畫著一條紅線障礙物。若有，則說明前方座標正確，再將擋板橫擺在 D1 正後方，若 rviz 中同樣在 D1 正後方橫畫著一條紅線障礙物，則 D1 前後方向已經正確。若實際中擋板的位置與rviz顯示的位置不一致，則根據對前面 引數調整 的瞭解，相對應地對引數進行調整。具體操作如下：擋板橫擺在 D1 正前方，rviz 顯示應該如下圖：

但rviz實際顯示，如下圖所示：

Ctrl + C ，關閉 rviz ， 也關閉正在執行的launch檔案，開啟該launch檔案。先將第 4 個引數改大（注意數值範圍），儲存launch檔案，接著啟動launch檔案並開啟rviz，檢視效果。若顯示的紅線向正確的紅線位置靠近，則再將引數慢慢加大以修正；若偏離正確位置，則將引數改小來修正。正前方的位置校正後，將擋板橫擺在 D1 正後方，rviz顯示與實際情況是相符的。

左右方向校正，只需要修改第 5 個引數。先將擋板橫擺在 D1 正左方，擋板中心與 D1 中心成一條直線，擋板的面要平行於 D1 正前方與正後方所成的直線。然後，對照 rviz 中 F4 的掃描顯示，D1 正左方是否橫畫著一條紅線障礙物。若有，則說明左方座標正確，再將擋板橫擺在 D1 正右方，若 rviz 中同樣在 D1 正右方橫畫著一條紅線障礙物，則 D1 左右方向已經正確，不用修改引數。若實際中擋板的位置與 rviz 顯示的位置不一致，則修改第 5 個引數，若當前數值為 0.0 ，則修改成 3.1415926 ；若當前數值為 3.1415926 ，則修改成 0.0 即可。再執行 launch 檔案與 rviz 核對效果。