注：本篇博文全部源碼下載地址為：Git Repo。
1. 下載到本地后解壓到當前文件夾然后運行：catkin_make 編譯。
2. 源碼是在 Ubuntu14.04 + Indigo 環境下編寫。
一、概述
經過前面一系列博文的介紹，已經可以利用RViz上的相關按鈕控制和仿真實際的機器人Rob了，本篇解決如何通過MoveIt提供的用戶接口個性化的和機器人進行交互（如發送命令讓機器人到達某個位姿）的問題。如圖1所示，主要介紹圖中紫色橢圓圈出部分的用法，圖中可以看出MoveIt為用戶提供了多個接口類，本篇僅以 MoveGroupInterface 類為例進行介紹，這個類的官方接口說明請點擊這里。
圖1
二、功能說明及實現
本篇博文的源碼程序包下載地址在開篇處，新增的包的名字為：rob_moveit_interface，程序基本流程圖如圖2所示，首先定義了雙臂的接口名稱，然后利用隨機生成的位姿模擬我們的目標位姿，進行雙臂的運動學求解和運動規划，最后經過MoveIt和我們定義的control包發送給實際的機器人進行執行。
圖2
具體而言，需要包含的頭文件為：
#include <moveit/move_group_interface/move_group.h> #include <moveit/planning_scene_interface/planning_scene_interface.h>
定義了雙臂的接口名稱代碼段為：
// Get the arm planning group
moveit::planning_interface::MoveGroup l_plan_group("left_arm");
moveit::planning_interface::MoveGroup r_plan_group("right_arm");
雙臂關節運動速度系數設置如下所示，系數的調節范圍為0~1, 這里我們利用隨機數函數隨機生成一個系數。
//set Velocity Factor (0,1] double Velocity_Factor = (double)((rand()%(100-(1)+1)) + (1))/100; l_plan_group.setMaxVelocityScalingFactor(Velocity_Factor); r_plan_group.setMaxVelocityScalingFactor(Velocity_Factor);
最后運動規划和執行的代碼段為：
//excute
for(int i=0;i<5;i++)
{
L_Joint_goal.push_back(left_movement[i]*degTorad);
R_Joint_goal.push_back(right_movement[i]*degTorad);
}
l_plan_group.setJointValueTarget (L_Joint_goal);
r_plan_group.setJointValueTarget (R_Joint_goal);
l_plan_group.move();
r_plan_group.move();
L_Joint_goal.clear();
R_Joint_goal.clear();
然后我們編寫一個launch文件用於啟動這一節點，這個也很簡單，如下所示，命名為 rob_moveit_interface.launch。
<launch> <node pkg="rob_moveit_interface" name="rob_moveit_interface" type="rob_moveit_interface" output="screen"> <!--launch-prefix="xterm−e"--> </node> </launch>
至此基本完成了接口程序包的編寫，然后利用catkin_make進行編譯。
三、測試
下面我們只需測試接口程序是否能滿足我們的預定功能即可，首先起一個shell （Shell 1），設置環境變量后將MoveIt相關包起來，具體命令如下，這里起的是我們在博文系列三中的所有程序包。
roslaunch rob_moveit_config demo.launch
然后重新起一個shell（Sheel 2），啟動我們本篇博文編寫的節點，命令如下：
roslaunch rob_moveit_interface rob_moveit_interface.launch
此時，我們看Sheel 2 的log發現程序已經產生了模擬的目標位姿並發送給MoveIt進行運動規划，主要 log 如下：
[ INFO] [1527688202.991717070]: Left Planning Reference frame: /base_link [ INFO] [1527688202.991861418]: Left EndEffector Reference frame: l_grasping_frame [ INFO] [1527688202.991926842]: Right Planning Reference frame: /base_link [ INFO] [1527688202.992057268]: Right EndEffector Reference frame: r_grasping_frame left_movement:[117.000000][42.000000][24.000000][47.000000][-11.000000] right_movement:[38.000000][36.000000][-26.000000][58.000000][-72.000000] Velocity_Factor = [0.360000] [rob_moveit_interface-1] process has finished cleanly
同時 Shell 1 中出現了雙臂的運動規划結果消息並發送給機器人，主要log如下，我們發現最后執行的位姿和我們生成的目標一致。
[ INFO] [1527688203.521864124]: I sent [38.00][36.00][-26.00][58.00][-72.00][0.63] [ INFO] [1527688203.521992769]: Right arm MSG [38.004787][35.999733][-25.999399][58.003475][-72.001976][0.631444] [ INFO] [1527688203.274094561]: I sent [117.00][42.00][23.99][47.00][-11.01][0.41] [ INFO] [1527688203.274198912]: Left arm MSG [116.997490][42.003925][23.994616][47.003212][-11.005501][0.413423]
最后，我們看到機器人在Rviz環境中執行了相關的動作，如圖3所示。
圖3
至此，我們完成了接口節點的編寫和測試，經過博文一到四，我們基本走通了：用戶接口輸入-->MoveIt運動規划-->運動消息通過Controller發送的基本功能，這里只是很淺顯的講了MoveIt的使用和配置過程，下一篇博文來介紹機械臂運動學相關的知識，並編寫一個簡單的運動學求解算法嵌入接口程序。
<-- 本篇完 -->
歡迎留言、私信、郵箱、微信等任何形式的技術交流。
作者信息：
名稱：Shawn
郵箱：zhanggx0102@163.com
微信二維碼：↓
