使用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
  ……
 #这是一个关于action的库，使用action必须加
  actionlib    
 #action的本质是基于消息的，因此，还需要添加moveit的相关消息
moveit_msgs
  ……
)

catkin_package(
  INCLUDE_DIRS include

  CATKIN_DEPENDS  …… actionlib  moveit_msgs ……

)

以下是package.xml文件（formate 2 格式）

<build_depend>actionlibbuild_depend>
<build_depend>moveit_msgsbuild_depend>

<exec_depend>actionlibexec_depend>
<exec_depend>moveit_msgsexec_depend>

（2）添加action服务端头文件

//action服务端的相关定义，请加入到驱动节点的头文件中
#include "actionlib/server/action_server.h"    

//action服务端的目标控制句柄定义，与接收的目标相关联后，可以用来实现action的信息反馈等操作       
#include "actionlib/server/server_goal_handle.h" 

（3）定义一个服务端对象

//定义action服务端
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>  as_;    

//定义action服务端目标控制句柄
actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_; 

 //用来反馈action目标的执行情况，客户端由此可以得知服务端是否执行成功了
control_msgs::FollowJointTrajectoryResult result_;         

注意control_msgs::FollowJointTrajectoryAction这个类型，该类型在上一篇文章中提到过，这是一个ROS自带的针对机械臂关节角的action类型，moveit作为action客户端也是这个类型，因此，这里编写的服务端必须保持类型一致。

（4）初始化这个服务端对象
先来看看官网是怎么初始化action服务端对象的，

官网中，action的实现是在一个类里，as_作为这个类的一个成员变量，as_的初始化写在了该类构造函数的初始化列表里(即构造函数冒号后面的内容），这一点很关键，因为服务端对象as_的初始化要先于包含它的类，因此，as_的初始化写错位置是要报错的。

假设你也是基于一个类来实现action的，假设这个类的名字叫做AuboDriver，那么AuboDriver的构造函数基本形式应该是这样的：

AuboDriver::AuboDriver():
  as_(nh, "aubo_i5/follow_joint_trajectory",
      boost::bind(&AuboDriver::goalCB, this, _1),
      false)
{
  as_.start();       //action服务端启动

}

仔细对比一下，这个写法和官网示例程序的写法是基本一致的，as_的构造函数参数基本含义如下：
nh：action所在ros节点的节点句柄名称。

“aubo_i5/follow_joint_trajectory”：这是action的名字，是action服务端与客户端配对的依据，在前一篇文章中讲的，controllers.yaml中写的参数name和action_ns共同决定了客户端action的名字，因此，服务端的名称必须参照controllers.yaml中的参数来写才能配对。

boost::bind(&AuboDriver::goalCB, this,_1)：boost这个是固定用法，就不深究了，它绑定了一个名为AuboDriver::goalCB的函数，AuboDriver是as_所属的类的名称，goalCB（）是其中的一个成员函数，这个函数写在了as_构造函数的第三个参数位，表示它是一个接受action目标的回调函数（函数名可以自己更改），当action客户端请求一个动作时，这个动作被认为是一个目标，传递给action服务端，此时就moveit的action客户端而言，这个目标就是机械臂运动轨迹，服务端接收到目标后，会自动回调goalCB这个函数，而目标（轨迹）会作为参数传递进来。

（5）写一个回调函数。
上一步中出现了一个名为goalCB()的回调函数，那么，接下来就是要具体编写这个回调函数，它是真实机械臂能够动起来的关键环节。

void AuboDriver::goalCB( actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
{

//疑点1：什么要将形参复制一份？
  actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *gh.getGoal();    //make a copy that we can modify

 //将之前定义的句柄与传入的action目标绑定在一起，
 goal_handle_ = gh;   


//疑点2：下面这个函数的作用是什么？
  reorder_traj_joints(goal.trajectory);

//添加路点函数，请参看文章开始的预备条件
  Add_waypoint(goal.trajectory);


 //执行轨迹函数，请参看文件开始的预备条件

 if（TrackMove()） //假设轨迹成功后返回值为1，失败时返回值为0
{
 //告诉客户端成功了
  goal_handle_.setAccepted();    
  result_.error_code = result_.SUCCESSFUL;

  goal_handle_.setSucceeded(result_);
}


}

上述程序示例里标注了两个疑点需要回答一下：

关于疑点1：为什么要将形参复制一份？
ROS下的回调函数参数的写法大家应该见了很多了，都是写成const……的形式，这也是C++传参所推荐的一种形式（常量指针），目的就是避免复制参数而带来的额外开销，但这里，我们给函数传递参数时没有使用const 常量指针的形式，这样直接传递参数实际上就是复制了一份参数，这显然带来了复制开销，为什么要这样做呢？因为我们要修改这个参数，但又不能直接改形参所指向的那个目标对象，那样做会破坏原始数据，是编程所不推荐的，这里复制一个副本，目的就是要修改这个数据而不破坏原始数据。

关于疑点2：reorder_traj_joints这个函数的作用是什么？
这个函数是需要用户自己写的，作用就是修改上一个疑点中提到的那个形参副本，好好的形参传递进来，为什么要修改？因为它内部数据顺序不对！

我们用goal这个变量存储了action客户端发过来的目标，这个目标是什么？是一个类型为control_msgs::FollowJointTrajectoryAction的action类型，它代表一组路点。也许有好几百个路点，每个路点记录了一组关节角（对于6自由度的机械臂就是joint1-joint6），但是，它的数据结构绝非这么简单，详细结构请查阅control_msgs::FollowJointTrajectoryAction的定义，其中每个关节角还有相对应的名字，这些关节名称是与最开始的机械臂xacro模型、以及后来的controllers.yaml中的关节名称是一脉相承的，你想要获取一个路点的关节角，可以这样追溯：goal.trajectory.points[i]，但是角标i绝不意味着是第i个关节，而是对应于goal.trajectory.joint_names[i]所指向的关节，因此，你必须先找到关节名字，再找对应的关节角，否则很可能发生张冠李戴的事情，而这里建议编写的reorder_traj_joints（）函数，就是将原有数据进行一次重映射排序，使得当我们使用下标来读取关节角时，所对应的角度与我们设想的1-6关节一一对应，你完全可以使用自己的手段来将形参中的数据与真实关节角对应起来，这里只是想强调这种现象，如果你忽视了关节角与关节名称相对应这个要点，很可能导致机械臂运动出现意想不到的后果，轻则机械报警不运动，重则机械臂打到周围的人或物，因此，请通过屏幕打印等多种调试手段来详细验证你的数据顺序是否接收正确。

剩下的代码就是具体机械臂与驱动相关的执行函数，当执行成功后，我们通过之前定义的goal_handle_来反馈执行成功的消息，告诉action客户端，你发的轨迹请求已经成功执行了。

梳理一下，本篇编写一个action服务端的主要思路：
1、添加action的依赖项、定义action对象，确保通过编译这一关；
2、实现action的一个回调函数，正确读取所传递过来的数据，确保通过信息传递这一关；
3、通过用户自己的驱动来执行action的目标，确保通过执行这一关。

到此，基本就能实现使用moveit控制真实机械臂的运动。