使用moveit！控制真实机械臂（5）——编写真实机械臂节点中的action服务程序

上一篇文章中，我们简要介绍了ROS中action的基本知识，为了控制真实机械臂，我们已经修改了moveit配置文件，使得moveti启动后具备一个control_msgs::FollowJointTrajectoryAction类型的action客户端，接下来，我们完成该类型action的另一半，编写C++程序来实现action的服务端。由于底层机器人控制程序一般差别都很大，这里展示的只是编程要点。

预备条件：

1、机械臂底层驱动已经完成。
无论你的机械臂是从大牌公司买来的还是自己造的，请预先完成好底层驱动控制部分，这里假设你的驱动控制程序不包括ROS相关代码，没有消息、没有action等等这些ROS才有的东西，仅仅是用C++语言控制机械臂的基本运动（比如运动到指定关节角、机械臂末端运动到指定位置和姿态）。这很重要，因为我们的目标是使用moveit控制真实机械臂，如果底层驱动还有问题，那后续内容就无从谈起了。

2、机械臂能够按照路点（轨迹）运动。
这应该是一般机械臂的一项基本功能。这里的轨迹由一个个路点组成，路点可以理解为机械臂在空间中运动时必须通过的点，一个个路点串起来就形成了轨迹。

路点的数据结构一般是一组关节角度，比如6自由度机械臂，有6个独立关节，其确定了机械臂在空间的唯一位置，那么，就可以用（joint1，joint2，joint3，joint4，joint5，joint6）来表示一个路点。为什么要求机械臂能够按照路点来运动？因为moveit最终发出来的控制指令的实质就是一连串的路点，只要机械臂能够执行这一串路点，就达到了机械臂按照规划的轨迹运动的目的，（如果你的机械臂不具备执行路点的功能，你可能需要自行转化moveit发送出来的数据）。

请将你的机械臂控制指令抽象为下面两个函数，你给函数起什么名字无所谓，只要具备下述功能即可。

驱动函数1： add_wayPoint();    
驱动函数2：trackMove();

函数1用来添加路点，假设你将所有即将执行的路点存在了一个“容器”里。
函数2用来执行这个“容器”里的路点，即按照一定规则进行差值，形成一条空间轨迹，让机械臂沿着轨迹动。

下面很多代码都是针对这样的抽象，单纯复制是行不通的，只有理解后将自己的程序改造成类似的结构才行。

如果你使用的是UR、Aubo这类商品化的机械臂，那么，上面所谓的预备条件都是天然满足的，这些功能厂家都集成在了相应的库函数里，是不需要用户操心具体实现的，你只需要关心给这些库函数传递什么数据即可。

具体改造过程：

1、用ROS封装你的驱动程序。
所谓用ROS封装，就是将你的普通C++程序改造成一个ROS节点程序，封装比较零碎，但其实很简单，这部分内容可以参照ROS教程beginner部分。原本你运行的是一个由C++驱动程序，现在这个文件里加入了ROS成分，变成了一个ros节点，就这么简单。

此时，你的机械臂驱动程序应该以及能够作为一个ROS节点运行了，即使现在这个节点没有订阅或发布什么话题，但它确实已经是一个ROS节点了，下面才是主要内容……

2、加入action服务端代码
（1）cmakelist.txt和package.xml中加入action相关依赖
以下是cmakelist.txt文件

find_package(catkin REQUIRED COMPONENTS
  ……