ROS slam gmapping不能畫地圖
最後啟動KobukiRos物件的update()函式，用於檢測底盤的各種狀態，比如底盤的連線狀態，電池狀態，輪子是否接觸地面等等。
Turtlebot的執行機制總結如下：
啟動minimal.launch之後，啟動Kobuki_nodelet節點，新建兩個類物件，一是用於底盤和ROS通訊類KobukiRos（橋樑），一個是底盤的驅動類Kobuki（心臟）。
通訊類中定義了用於接收ROS命令話題的接收者，接收如/cmd_vel等話題。接收者接收到話題訊息後，通過回撥函式，將ROS通用格式的資料轉換為底盤可用格式的資料，並呼叫驅動類的函式介面，通過串列埠給底盤單片機發送資料，實現對底盤的操控。
通訊類中還定義了用於釋出底盤狀態話題的釋出者，如傳送/sensors/imu_data的話題。通訊類通過驅動類的函式介面獲得原始感測器資料，將其轉換為ROS通用資料格式，由釋出者傳送給ROS上層，來實現反饋功能。
最後，Kobuki_nodelet啟動了一套完整的監測機制，用於監測底盤的各種異常狀態。
參考原始碼：
kobuki_nodelet.cpp:（實現Kobuki_nodelet類）
kobuki_ros.hpp:
kobuki_ros.cpp：（實現KobukiRos類，相當於橋樑）
subscriber_callbacks.cpp:（實現回撥函式，ROS資料格式和底盤資料格式的轉換）
kobuki.hpp:
kobuki.cpp:（實現Kobuki類，驅動類，相當於心臟）
詳細跳轉機制：（以/cmd_vel為例）
在Kobuki_nodelet.cpp中，首先新類KobukiRos物件。然後呼叫KobukiRos的init()初始化函式，在這個函式中，聲明瞭許多釋出者和訂閱者，下面以/cmd_vel話題為例，繼續深入。
velocity_command_subscriber= nh.subscribe(std::string("commands/velocity"), 10,&KobukiRos::subscribeVelocityCommand, this);
當有/cmd_vel話題釋出時，這個釋出者的回撥函式自動被呼叫，即進入KobukiRos::subscribeVelocityCommand（）中。
在回撥函式中，其將資料傳送給驅動類Kobuki的函式中：
kobuki.setBaseControl這個函式通過呼叫diff_driver類（差速輪驅動）的setVelocityCommands()函式，將/cmd_vel話題的Twist型別資料轉換為左右輪速，並儲存在vector容器中。
注意，diff_driver::setVelocityCommands()並沒有直接將左右輪速通過串列埠傳送給底盤，而是儲存在vector<double>型別的私有成員point_velocity之中。
為什麼沒有直接通過串列埠傳輸呢，那傳輸步驟在哪呢？我們接著來看，以上的步驟其實僅僅是一個執行緒，我們再來看看另外的執行緒。
繼續回到KobukiRos的init()初始化函式，除了聲明瞭釋出者和訂閱者用於接收發布資料，同時也呼叫了驅動類Kobuki的init（）初始化函式。
在驅動類Kobuki的init()初始化函式中，初始了串列埠，並設定了一些必要資訊，最後進入了Kobuki::spin()迴圈函式。
在spin()函式中，首先判斷了是否打開了串列埠，如果沒開啟就開啟並設定串列埠。確認完畢之後，先做讀操作，然後做寫操作。在寫操作中，呼叫了sendBaseControlCommand()，在這個函式中，先呼叫了上面講過的diff_driver：：velocityCommands()函式，將轉換好的資料設定在Kobuki類的vector<double>型別的私有成員velocity_commands中。最後呼叫sendCommand()函式，終於在裡面看到了串列埠寫操作的相關程式碼！！！
4.運動規劃 (Motion Planning): MoveIt! 與 OMPL
最近有不少人詢問有關MoveIt!與OMPL相關的話題，但是大部分問題都集中於XXX功能怎麼實現，XXX錯誤怎麼解決。表面上看，解決這些問題的方法就是提供正確的程式碼，正確的編譯方法，正確的執行步驟。
然而，這種解決方法只能解決這個特定的問題，而且解決之後我們也無法學到一些實際的東西。要想徹底明白，需要從源頭入手，也就是說，不要問“MoveIt! 怎麼把機械手從空間一個點移到另一個點？“，而是要問”MoveIt!
為什麼能把機械手從空間一個點移到另一個點？“。這一點明白之後，遇到類似的問題，才能從容應對。同理，這不僅適用於MoveIt，也同樣適用於其他任何ROS功能。
所以，下文中我們會見到一些具體的例子，但整體上，更傾向於巨集觀的概念和一些基礎的方法，希望對大家能有所幫助。這裡的幫助指的是增強對運動規劃和Moveit, OMPL的整體理解，而非侷限於完成某一個功能，編譯執行某一個檔案。
我嘗試用最簡單，最通俗的表達方式來解釋這些問題，其中不免會有一些學術上的錯誤用詞和解釋，請專業的朋友們見諒，也歡迎指出錯誤。
一. 基礎概念
首先，我們要了解一些基礎的概念，瞭解各個名詞的意義和區別。
1.1. 運動規劃 (Motion Planning)
我們這裡講的運動規劃，有別於軌跡規劃 (Path
Planning)。一般來說，path planning用於無人車/無人機領域，而motion planning主要用於機械臂，類人機器人領域。當然了，這兩者沒有本質的區別，理論上說MoveIt!和OMPL同樣可以用於無人車無人機的規劃，但不免有些殺雞用牛刀的感覺。兩者規劃的空間維度不同，導致他們的難易程度不同。舉例說明，如果不考慮速度加速度，只考慮位置的話，無人車軌跡規劃維度是3 (x,y,和角度), 無人機是6 (x,y,z,和另外3個量確定空間的旋轉角度)。確定3D空間的一個姿勢(pose)需要6個變數，而對於關節數大於6的機械臂結構，它的規劃空間維度就大於6，成為冗餘系統(redundant
system)，從而使規劃問題變得更為複雜。所謂冗餘系統，就是說，存在多種關節角度配置能夠使得終端達到相同的位姿，存在無數的解。這是達到的最終姿勢有無數個解，那麼如何到達這個最終姿勢，整個運動的軌跡，更是存在無數個解。
既然存在無數的解，那麼問題來了。很明顯，存在兩種不同的方向，一種是找到最好的那個解，另一種是快速的找到一個有效的解。前者，大部分演算法使用最優規劃 (Optimization-based Planning)，後者使用取樣規劃 (Sampling-based Planning)。具體的區別和演算法，不在這裡贅述。
1.2. 開源運動規劃庫 (OMPL).
接上文，而OMPL (Open Motion Planning Library)， 開源運動規劃庫，就是一個運動規劃的C++庫，其包含了很多運動規劃領域的前沿演算法。雖然OMPL裡面提到了最優規劃，但總體來說OMPL還是一個取樣規劃演算法庫。而取樣規劃演算法中，最出名的莫過於
Rapidly-exploring Random Trees (RRT)
和 Probabilistic Roadmap (PRM)了,
當然，這兩個是比較老的，還有很多其他新演算法。
- OMPL能做什麼？ 簡單說，就是提供一個運動軌跡。給定一個機器人結構(假設有N個關節)，給定一個目標(比如終端移到xyz)，給定一個環境，那麼OMPL會提供給你一個軌跡，包含M個數組，每一個數組長度是N，也就是一個完整的關節位置。沿著這個軌跡依次移動關節，就可以最終把終端移到xyz，當然，這個軌跡應當不與環境中的任何障礙發生碰撞。
- 為什麼用OMPL？ 運動規劃的軟體庫和演算法有很多，而OMPL由於其模組化的設計和穩定的更新，成為最流行的規劃軟體庫之一。很多新演算法都在OMPL開發。很多其他軟體（包括ROS/MoveIt）都使用OMPL做運動規劃。
1.3. 逆運動學 (Inverse Kinematics)
- 什麼是逆運動學(IK)？簡單說，就是把終端位姿變成關節角度，q=IK(p)。p是終端位姿(xyz)，q是關節角度。
- 為什麼要用IK？OMPL是取樣演算法，也就是要在關節空間取樣。 這與無人車的規劃有一個最明顯的區別，無人車的目標就是在取樣空間， e.g. 目標是(x,y), 取樣空間也是(x,y). 但是對於機械臂，目標是終端空間位置(xyz), 但取樣空間卻是關節空間(q0,q1,...qN)。有了IK之後，我們就可以把三維空間的目標p轉化為關節空間的目標q。那麼這樣就會讓取樣演算法能算的更快，具體方法不贅述，這樣的演算法有RRT-Connect，BKPIECE等等雙向取樣演算法。
問：我不想看也看不懂OMPL和各種演算法，但是我想讓機械臂動起來，怎麼辦？
答：那這正是MoveIt!的設計初衷。Move It！讓它動起來！
OMPL是運動規劃的“規劃”部分，而MoveIt!是OMPL的ROS介面。當然這不完全準確，OMPL有單獨的ROS介面，但依舊很繁雜，而MoveIt是OMPL ROS介面的介面。。。而且MoveIt!還結合了其他一些功能，總之MoveIt!就是個大介面。。
- MoveIt!能做什麼？一句話，MoveIt!就是一個模組化的介面，讓你在最短時間內，不用自己寫太多程式碼，就能配置出一個ROS Package來為你的機械臂做運動規劃。
二. 建立MoveIt! Package
2.1 準備URDF package
首先我們要準備一個機械臂的urdf，如果你已有URDF，可以使用自己的urdf模型。若手頭沒有現成的URDF，可以從此處下載一個庫卡LWR簡化模型URDF，這是一個固定底座7自由度的機械臂。
從該連線處依次進入examples/sovlers/ik_solver_demo/resources，下載裡面的lwr_simplified.urdf。
複製程式碼
| 1 2 3 4 |
cd
path_to_catkin_ws/src
catkin_create_pkg
lwr_description
cd
lwr_description
mkdir
urdf
|
將下載好的lwr_simplified.urdf放入urdf資料夾中，這樣一個urdf package便建立好了。
2.2 MoveIt!配置助手 (MoveIt! Setup Assistant)
2.2.1 開啟MoveIt! Setup Assistant
複製程式碼
| 1 |
roslaunch
moveit_setup_assistant setup_assistant.launch
|
MoveIt! Setup Assistant 是一個圖形介面，可以讓我們不用寫程式碼看程式碼，直接用滑鼠點選就可以配置機器人的運動規劃所需要的資訊。點選Create New MoveIt Configuration Package來建立新的配置包，選擇剛剛下載的urdf，然後點選Load Files 載入檔案。
2.2.2 建立碰撞免檢矩陣(ACM)
點選Setup Assisant的左邊第二項'Self-Collisions'，在這裡我們將建立碰撞免檢矩陣(Avoid Collision Matrix, ACM)。再次強調，怎麼建立很簡單，點選一下'Regenerate Default Collision Matrix'就可以了，問題是，為什麼？ACM是做什麼的？
我們知道，碰撞檢測是非常複雜的運算過程。對於多關節機械臂或者類人機器人來說，機械結構複雜，肢體多，碰撞檢測需要涉及很多的空間幾何計算。但是對於剛體機器人來說，有些肢體之間是不可能發生碰撞的，比如原本就相鄰的肢體，比如類人機器人的腳和頭。這裡生成的ACM就是告訴我們，這個URDF所描述的機器人，哪些肢體之間是不會發生碰撞的。那麼在之後的碰撞檢測演算法中，我們就可以略過對這些肢體之間的檢測，以提高檢測效率。
2.2.3 建立虛擬關節 (Virtual Joints)
在Setup Assistant 第三項Virtual Joints裡面，我們要建立所謂的虛擬關節。這個虛擬關節，可以理解為一個連線機器人和世界的關節。
一般來說，Virtual Joint Name我們命名為‘world_joint’，而'Child Link'指的是我們要把‘世界’和機器人的那個部位連線，那麼很顯然我們選擇基座'base'。‘Parent Frame Name’，是世界座標的名字，在ROS中一般叫'world_frame'。關節型別 Joint Type, 很顯然這裡我們選擇固定Fixed. 代表機器人相對於世界是固定的。而另外兩種, Planar指的是平面移動底座(xy平面+角度)，用於移動機器人比如PR2；
還有一種Floating, 指的是浮動基座(xyz position+orientation)，比如類人機器人。 2.2.4 建立規劃群 (Planning Groups) 建立Planning Group是MoveIt的核心之一。首先，點選Add Group, 我們會看到一個介面，如下圖，
那麼這些都是什麼呢？
- Group Name: 不用多說，名字。。。我們就叫Arm。
- Kinematic Solver: 運動學求解工具，這個就是負責求解正向運動學(Forward Kinematics)和逆運動學(IK, 見1.3節)的。 一般我們選用KDL， 。這是一個運動學與動力學的庫，可以很好的解決6自由度以上的單鏈機械結構的正逆運動學問題。當然你也可以用其他IK Solver, 比如SRV或者<span \"="" style="box-sizing: border-box;">IK_FAST，甚至你可以自己開發新的Solver然後插入進來，如果有空，我以後會發帖講解如何建立新的運動學求解庫並插入到MoveIt。
- Kin. Search Resolution: 關節空間的取樣密度
- Kin. Search TImeout: 求解時間
- Kin. Solver Attempts: 求解失敗嘗試次數，一般來說這三項使用預設值就可以。你也可以根據具體需要做出適當調整。
在正中方我們可以看到這個機械臂的結構，一個link接著一個link。下方我們可以看到有'Base Link'和'Tip Link'，我們選擇'lwr_arm_0_link'作為Base，選擇'lwr_arm_7_link'作為Tip. 然後點選Save，這樣一個規劃組群就建立好了。同樣的，我們可以再建立一個手的組群(Hand)，這一次我們用Add Links，然後選擇'lwr_arm_7_link'。
2.2.5 建立機器人預設位姿 (Robot Poses)
2.2.6 配置終端控制器(End Effectors)
2.2.7 配置被動關節(Passive Joints)
所謂被動關節，就是指現實中不配置電機的關節，也就是不會出現在機器人的Joint State Msg裡，以避免MoveIt與JointState出現匹配錯誤。這裡我們的LWR機械臂並沒有此類被動關節，所以可以直接跳過。
2.2.8 生成配置檔案(Configuration Files)
最後一步，在Configuration Package Save Path裡面選擇一個儲存地址，一般我們把他放在path_to_catkin_ws/src/lwr_moveit_config然後點選Generate Package，這樣一個完整的MoveIt Configuration Package就建立好了！先不要急著執行，我們先來看看都生成了哪些東西，還有一些重要的配置引數都是在哪定義的。
三. MoveIt 配置包詳解
開啟剛剛建立好的lwr_moveit_config資料夾，我們發現有config和launch兩個資料夾。3.1 MoveIt! 配置檔案先看config，裡面有
- fake_controllers.yaml：這是虛擬控制器配置檔案，方便我們在沒有實體機器人，甚至沒有任何模擬器（如gazebo）開啟的情況下也能執行MoveIt。
- joint_limits.yaml：這裡記錄了機器人各個關節的位置速度加速度的極限，這些都會被用於以後的規劃中。
- kinematics.yaml：這裡就是上一章2.2.4裡面設定的東西，用於初始化運動學求解庫
- lwr.srdf：這個是一個重要的MoveIt配置檔案，我們將在下一節詳解。
- ompl_planning.yaml：這裡是配置OMPL各種演算法的各種引數。
3.2 SRDF檔案
SRDF是moveit的配置檔案，配合URDF使用。開啟lwr.srdf，
我們可以看到這是一個xml格式的配置檔案，根是robot，並有一個屬性值name='lwr'。下面各個專案應該很明顯，就是我們剛剛在Setup Assistant裡面所設定的東西，包含了組群，位姿，終端控制器，虛擬關節，以及碰撞免測矩陣ACM的定義。理論上，只要有了srdf和urdf，我們就可以完全定義一個機器人moveit資訊。 3.3 Launch檔案
下面，我們看看launch資料夾，一打開發現有很多檔案，瞬間不想看了。。不要急，我們來看看幾個重要的檔案。
3.3.1 demo.launch
demo是執行的總結點，開啟我們可以看到他include了其他的launch檔案。其中第14行說，如果有需要，釋出靜態的tf。比如說，你的機器人基座不在世界座標的原點，你可以釋出一個靜態tf來描述機器人在世界座標中的位置。第17-21行，就是我們釋出虛擬機器器人狀態的地方了，當然，如果你有實體機器人或者有gazebo之類的模擬器，你需要去掉這一部分，有其他相應的節點來發布機器人狀態。26-32行運行了另一個moveit重要的節點，move
group。
3.3.2 move_group.launch
顧名思義，move group的功能是讓一個規劃組群動起來。怎麼動，那就要做運動規劃了，在move_group.launch第24-26行定義了運動規劃庫的使用，我們可以看到，預設的是使用ompl運動規劃庫。同樣的，如果以後有時間，我會發帖詳解如何建立新的運動規劃庫外掛並讓moveit使用其他的運動規劃演算法。其他的都是設定一些基本引數，暫時可以略過。
3.3.3 planning_context.launch
這裡我們可以看到，定義了所使用的urdf和srdf檔案，以及運動學求解庫。不建議手動更改這些，但是如果你需要使用不同的urdf，srdf，可以在這裡更改。
3.3.4 setup_assistant.launch
如果你需要更改一些配置，那麼可以直接執行
複製程式碼
| 1 |
roslaunch
lwr_moveit_config setup_assistant.launch
|
這樣就可以基於當前設定做更改，而不是重新設定。
四. 執行MoveIt!
4.1 Launch Demo
現在我們可以來嘗試執行moveit了！
複製程式碼
| 1 |
roslaunch
lwr_moveit_config demo.launch
|
等待幾秒，當看到 All is well! Everyone is happy! You can start planning now! 的時候，就代表啟動成功了。我們可以看到一個Rivz視窗，左下角有一個運動規劃MotionPlanning模組。
第一個進入視野的就是Planning Library, OMPL。沒錯，這裡告訴你當前用的是OMPL運動規劃演算法。在中間的下來選單裡面有很多的具體演算法，之後你可以嘗試不同的演算法，看看他們的區別。 4.2 選擇目標位姿 如果如上文第二章中設定，你會在rviz主視窗中看到一個互動標記位於機械臂終端位置。移動這個標記到另外一個地方，你可以看到一個橙色的目標位姿(每一次移動標記，就運行了一次逆運動學IK求解過程)。
同樣的，你也可以在MotionPlanning模組下的Planning子模組寫的Query子模組裡面設定隨機的或者預設的目標位置。
4.3 運動規劃終於，到了運動規劃的時候了。。在Planning子模組中單擊Plan，一個運動軌跡就會出現與Rviz視窗中並迴圈播放。你可以在Display->MotionPlanning->Planned
Path裡面設定各種顯示引數。
同時，在terminal裡面我們可以看到一些輸出
複製程式碼
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
[
INFO] [1453481861.884163555]: LBKPIECE1: Attempting to use default projection.
[
INFO] [1453481861.884336258]: LBKPIECE1: Attempting to use default projection.
[
INFO] [1453481861.884489778]: LBKPIECE1: Starting planning with 1 states already in datastructure
[
INFO] [1453481861.884523826]: LBKPIECE1: Attempting to use default projection.
[
INFO] [1453481861.884547702]: LBKPIECE1: Starting planning with 1 states already in datastructure
[
INFO] [1453481861.884564358]: LBKPIECE1: Attempting to use default projection.
[
INFO] [1453481861.884587404]: LBKPIECE1: Starting planning with 1 states already in datastructure
[
INFO] [1453481861.884604829]: LBKPIECE1: Attempting to use default projection.
[
INFO] [1453481861.884626253]: LBKPIECE1: Starting planning with 1 states already in datastructure
[
INFO] [1453481861.905034917]: LBKPIECE1: Created 99 (46 start + 53 goal) states in 88 cells (45 start (45 on boundary) + 43 goal (43 on boundary))
[
INFO] [1453481861.905633020]: LBKPIECE1: Created 87 (33 start + 54 goal) states in 76 cells (31 start (31 on boundary) + 45 goal (45 on boundary))
[
INFO] [1453481861.913846457]: LBKPIECE1: Created 126 (76 start + 50 goal) states in 115 cells (75 start (75 on boundary) + 40 goal (40 on boundary))
[
INFO] [1453481861.914639489]: LBKPIECE1: Created 220 (72 start + 148 goal) states in 201 cells (70 start (70 on boundary) + 131 goal (131 on boundary))
[
INFO] [1453481861.948016518]: ParallelPlan::solve(): Solution found by one or more threads in 0.063719 seconds
|
複製程式碼
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
|