注：本篇博文全部源碼下載地址為：Git Repo。
1. 下載到本地后解壓到當前文件夾然后運行：catkin_make 編譯。
2. 源碼是在 Ubuntu14.04 + Indigo 環境下編寫。
一、概述
本系列博文的第一篇，我們完成了雙臂機器人 Rob 的建模工作；第二篇博文則詳細介紹 MoveIt 模塊的配置工具 Setup Assistant Tool 的使用，並配置了一個簡單的使用環境。具體在程序結構上如圖1所示，我們已經完成了兩處紅色橢圓圈出的部分，即完成了ROS Param Server （建模）的工作，同時利用系統工具配置了move_group節點。
圖1
但是，到目前為止我們仍然只是在模擬環境中（Rviz）看到我們的機器人做動作，仍然沒有和現實中的機器人產生關聯，如何利用這個框架控制現實中的機器人則是本博文需要解決的問題。具體到程序結構上如圖1所示，我們要完成綠色橢圓圈出的兩個部分的工作，即將MoveIt的運動消息發送給實際的機器人，同時利用發布機器人的關節信息，實現方法如下文所述。
二、改造默認生成的 rob_moveit_config 包
我首先是直接對上一篇博文中生成的 rob_moveit_config 包進行改造，使其滿足我們的實際使用要求，具體過程如下：
1. 創建 controllers.yaml 文件。
首先在 src/rob_moveit_pack/rob_moveit_config/config 文件夾下，新建一個文件 controllers.yaml， 這個文件是機器人控制器的配置（定義）文件，我們創建它是用於代替在這個目錄下的 fake_controllers.yaml 文件的，這個控制器用於直接和我們的機器人進行交互。
然后打開 controllers.yaml 文件然后填寫以下內容：
#controller_manager_ns: controller_manager
controller_list:
- name: r_rob_mover
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- r_shoulder_joint
- r_bigarm_joint
- r_rotatearm_joint
- r_elbow_joint
- r_wrist_joint
- name: l_rob_mover
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- l_shoulder_joint
- l_bigarm_joint
- l_rotatearm_joint
- l_elbow_joint
- l_wrist_joint
這里我們定義了兩個控制器，分別是控制右臂的 r_rob_mover 和控制左臂的 l_rob_mover ，並分別制定了兩個控制器下面控制的關節名稱。注意，控制器名字我們自己定義，其余部分均為ROS默認的需要的配置，關節名稱需要和機器人模型文件（xacro）中的名稱一致。
2. 改寫 rob_robot_moveit_controller_manager.launch.xml 文件。
在 src/rob_moveit_pack/rob_moveit_config/launch 文件加下，找到 rob_robot_moveit_controller_manager.launch.xml 文件，這個文件用於配置 MoveIt 的控制器，我們需要將MoveIt 的控制器指向我們上一步創建的控制器，此文件內容改為如下：
<launch> <!-- Set the param that trajectory_execution_manager needs to find the controller plugin --> <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" /> <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/> <!-- load controller_list --> <arg name="use_controller_manager" default="true" /> <param name="use_controller_manager" value="$(arg use_controller_manager)" /> <!-- Load joint controller configurations from YAML file to parameter server --> <rosparam file="$(find rob_moveit_config)/config/controllers.yaml"/> </launch>
3. 改寫 demo.launch 文件。
具體來說修改一下幾個地方：
<1> 改寫 joint_state_publisher 節點啟動參數
<rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam> 改為 <rosparam param="/source_list">[/joint_states]</rosparam>；
具體話題 /joint_states 的由來詳見下文。
<2> 改寫 move_group.launch 文件的配置參數
<arg name="fake_execution" value="true"/> 改為 <arg name="fake_execution" value="false"/>
<3> 添加 rob_control 節點的啟動文件
具體 rob_control 節點的由來詳見下一章節。
修改完成的 demo.launch 文件如下所示：
<launch> <!-- By default, we do not start a database (it can be large) --> <arg name="db" default="false" /> <!-- Allow user to specify database location --> <arg name="db_path" default="$(find rob_moveit_config)/default_warehouse_mongo_db" /> <!-- By default, we are not in debug mode --> <arg name="debug" default="false" /> <arg name="use_gui" default="false" /> <!-- Load the URDF, SRDF and other .yaml configuration files on the param server --> <include file="$(find rob_moveit_config)/launch/planning_context.launch"> <arg name="load_robot_description" value="true"/> </include> <!-- If needed, broadcast static tf for robot root --> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> <param name="/use_gui" value="$(arg use_gui)"/> <!-- <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam> --> <rosparam param="/source_list">[/joint_states]</rosparam> </node> <!-- Given the published joint states, publish tf for the robot links --> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" /> <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) --> <include file="$(find rob_moveit_config)/launch/move_group.launch"> <arg name="allow_trajectory_execution" value="true"/> <arg name="fake_execution" value="false"/> <arg name="info" value="true"/> <arg name="debug" value="$(arg debug)"/> </include> <!-- Run Rviz and load the default config to see the state of the move_group node --> <include file="$(find rob_moveit_config)/launch/moveit_rviz.launch"> <arg name="config" value="true"/> <arg name="debug" value="$(arg debug)"/> </include> <!-- If database loading was enabled, start mongodb as well --> <include file="$(find rob_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"> <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/> </include> <!-- start rob control --> <include file="$(find rob_control)/launch/rob_control.launch" > launch-prefix="xterm -e" </include> </launch>
三、添加 rob_control 包
至此，我們需要添加一個 Package 用於創建一個新的節點（Node），這個節點我們起名為 rob_control , 他的作用是：1. 處理 MoveIt 產生的運動消息，並發送給實際的機器人；2. 接收機器人傳感器產生的實際運動參數，發布機器人的各個關節的關節狀態。添加完新的 package 后的程序包結構如下，紅色部分為新添加的包。
src
├── CMakeLists.txt
└── rob_moveit_pack
├── rob_description
├── rob_moveit_config
├── rob_control
│ ├── CMakeLists.txt
│ ├── include
│ │ └── rob_control
│ │ ├── rob_comm.h
│ │ └── rob_control.h
│ ├── launch
│ │ └── rob_control.launch
│ ├── package.xml
│ └── src
│ ├── rob_comm.cpp
│ └── rob_control.cpp
└── rob_moveit_pack
├── CMakeLists.txt
└── package.xml
這個包的程序結構如圖2所示，接收 Move_Group 模塊產生的機器人運動規划消息，然后通過一定的通信手段（如CAN通信、串口通信、TCP等）發送真正的給機器人，同時接收機器人的關節信息（機器人各個關節傳感器測得的實際關節數據），然后將這個數據發送發布到 話題 /joint_states 上，這就是為什么第二部分的第3小節中講到修改 demo.launch 文件中 joint_state_publisher 節點啟動參數時訂閱話題 /joint_states，只有這樣才能實時更新機器人的狀態，避免每次新的運動都從零狀態出發。
圖2
我們知道，ROS系統提供了一種具有搶占功能的CS（Client - Server）節點（node）通信方式，就是Actionlib，這里MoveIt 發布機器人運動消息序列就是采用這種通信方式（FollowJointTrajectoryAction
接口），以便機器人可以隨時更新狀態並覆蓋掉未執行的老的狀態。這個接口的使用方法在 rob_control.cpp 源碼中有詳細的使用方法，這里只做簡單說明。
<1> 首先做一個定義，方便后續使用
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> R_TrajectoryServer;
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> L_TrajectoryServer;
<2> 然后對其進行定義和初始化
//Start the ActionServer for JointTrajectoryActions from MoveIT R_TrajectoryServer r_tserver(n2, "r_rob_mover/follow_joint_trajectory", boost::bind(&r_executeTrajectory, _1, &r_tserver), false); ROS_INFO("TrajectoryActionServer: Starting"); r_tserver.start(); L_TrajectoryServer l_tserver(n2, "l_rob_mover/follow_joint_trajectory", boost::bind(&l_executeTrajectory, _1, &l_tserver), false); ROS_INFO("TrajectoryActionServer: Starting"); l_tserver.start();
<3> 回調函數編寫（以右臂為例）
// Processing and JointTrajectoryAction void r_executeTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr& r_goal, R_TrajectoryServer* r_as) { robotState rs; float lastDuration = 0.0; int nrOfPoints = r_goal->trajectory.points.size(); // Number of points to add for(int i=0; i<nrOfPoints; i++) { rs.j[0] = r_goal->trajectory.points[i].positions[3]*rad2deg; // ros values come in rad, internally we work in degree rs.j[1] = r_goal->trajectory.points[i].positions[0]*rad2deg; rs.j[2] = r_goal->trajectory.points[i].positions[2]*rad2deg; rs.j[3] = r_goal->trajectory.points[i].positions[1]*rad2deg; rs.j[4] = r_goal->trajectory.points[i].positions[4]*rad2deg; float dtmp = r_goal->trajectory.points[i].time_from_start.toSec(); rs.duration = dtmp - lastDuration;// time_from_start is adding up, these values are only for the single motion lastDuration = dtmp; r_targetPointList.push_back(rs); } r_as->setSucceeded(); //debug msg ROS_INFO("right arm recv: %f %f %f %f %f ,duration: %f", rs.j[0],rs.j[1],rs.j[2],rs.j[3],rs.j[4],rs.duration); }
這里rob_control 包給出了一個比較通用的程序框架，大家只需稍作修改就可以應在自己的機器人項目中了。
四、效果檢驗
編譯完成后，在workspace目錄下，運行source devel/setup.bash配置環境后直接運行：
roslaunch rob_moveit_config demo.launch
我們在Rviz環境中 MotionPlaning 窗口的 Planing 選項下點擊 Updata 隨機生成一個機器人姿態，然后分別點擊 Plan 和 Execute 按鈕，就完成了一次動作的運動規划，如圖3所示。
圖3
這個時候運動規划傳送的機器人關節運動序列就通過 rob_control 節點發送給機器人了，我們可以看到部分發送的debug log如圖4所示。
圖4
整個程序各個節點以及話題之間的鏈接關系如圖5所示，我們可以看到話題 /joint_states 的存在，我們就是通過它實時發布機器人的狀態的，進而 tf 才能正常運轉。
圖5
至此我們完成了MoveIt 核心程序的配置以及和我們機器人實現了通信鏈接，本文給出了一個比較通用的程序框架，后面我們進一步介紹MoveIt 的用戶接口的使用（User Interface ）和雙臂機器人的運動學相關知識。
<-- 本篇完 -->
歡迎留言、私信、郵箱、微信等任何形式的技術交流。
作者信息：
名稱：Shawn
郵箱：zhanggx0102@163.com
微信二維碼：↓
