ROS中Moveit生成的轨迹如何作用与实际的机械臂(二)

在<ROS中Moveit生成的轨迹如何作用与实际的机器人(一)>中，我们论述了moveit怎样产生机械臂的轨迹规划点，并创建了一个Action Server节点来接受轨迹点信息。接下来便是考虑用这些轨迹点来使一个实际的机器人运动了。

由于PC上缺乏直接的产生脉冲的设备，因此我们就需要再借助额外的硬件来完成。那么就有两种方案来实现。

1. 在Action Serve节点中进行细插补，再通过串口或网络实时（其实不太实时）地传递每一个脉冲指令给控制电路板，控制板可以是带linux系统的树莓派等，也可以是不带系统的stm32系统板等。只要能发出脉冲即可。
2. 将细插补下放到控制电路板中执行。Action Server节点将所有轨迹点一起打包发送给控制板即可。这样做简化了Action Server节点，也可以充分利用控制板的实时性，让运动更流畅。

我们采用树莓派来作为硬件控制器，在里面已经预先实现好了脉冲的发送程序，可以在指定的时间内发送指定数量的脉冲。那么现在就需要通过TCP通信将轨迹点信息从PC上获取过来。我们将树莓派作为TCP服务器端，PC上的Action Server节点作为TCP的客户端。所以树莓派上的TCP服务器端先启动进入侦听状态。然后PC上的Action Serve节点启动并连接到服务器，再将数据发送过来。

我们采用asio+protobuf的方案来完成客户端和服务器端的功能类。asio是boost提供的跨平台的网络及低级别I/O功能库，可以以非常简洁的方式来实现平台无关的TCP通信。protobuf则是google提供的平台无关、语言无关的用于数据交换的功能库，可以高效地将轨迹点信息进行序列化并进行传输

轨迹点的数据序列化

上面已经说过了使用protobuf来处理可变的轨迹点信息。首先按照protobuf的要求定义数据结构描述文件–jointtrajectory.proto，其内容如下：

syntax="proto3";
message Point{
	repeated double positions=1;
	repeated double velocities=2;
	repeated double accelerations=3;
	double time_frome_start=4;
}
message JointTrajectoryPoints{
	repeated Point points=1;
}

protobuf强大的地方就是可以优雅地描述可变的数据。这里轨迹点的数量以及每个轨迹点中各关节值数量都是不固定的。可以想象如果自己编写一个这样的数据通信会有一定的麻烦，且写出来的代码还不一定通用。描述文件中repeated就表示该数据是可变的。

完成数据结构描述后，就可以用protobuf提供的工具来生成数据结构相关的数据类：

$ protoc --cpp_out=./ jointtrajectory.proto

这样便在当前目录产生"jointtrajectory.pb.h"和"jointtrajectory.pb.cc"。里面包含JointTrajectoryPoints和Point类，其添加元素的操作很简单，对于非repeated的字段，有形如Point.set_fieldname(value)的函数设置该字段的值。而对于repeated字段，可以把它看成一个vector，有形如Point.add_fieldname()的函数往这个vector中添加一个元素，同时可以使用iterator的形式对其中的元素进行遍历。

由于Moveit产生的是trajectory_msgs::JointTrajectory数据类型，因此我们将其转换到JointTrajectoryPoints，具体代码如下：

void generateJointTrajectoryData(trajectory_msgs::JointTrajectory::_points_type &points, JointTrajectoryPoints &jtPts)
{
	Point *pts;
	trajectory_msgs::JointTrajectory::_points_type::iterator pt_iter;
	trajectory_msgs::JointTr