ROS中Moveit生成的轨迹如何作用与实际的机械臂(二)
在
由于PC上缺乏直接的产生脉冲的设备，因此我们就需要再借助额外的硬件来完成。那么就有两种方案来实现。
- 在Action Serve节点中进行细插补，再通过串口或网络实时（其实不太实时）地传递每一个脉冲指令给控制电路板，控制板可以是带linux系统的树莓派等，也可以是不带系统的stm32系统板等。只要能发出脉冲即可。
- 将细插补下放到控制电路板中执行。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::JointTrajectoryPoint::_positions_type::iterator pos_iter;
trajectory_msgs::JointTrajectoryPoint::_time_from_start_type tm_iter;
//pts->add_positions(iter->positions[0]);
for(pt_iter=points.begin(); pt_iter!=points.end(); pt_iter++) {
pts=jtPts.add_points();
//add all joint position to this point.
for(pos_iter=pt_iter->positions.begin(); pos_iter!=pt_iter->positions.end(); pos_iter++)
pts->add_positions(*pos_iter);
pts->set_time_frome_start(pt_iter->time_from_start.toSec());
}
}
然后将所得到的JointTrajectoryPoints对象jtPts序列化成二进制串，再发送给Tcp服务器即可。
jtPts.SerializeToArray(data, jtPts.ByteSize() );
cout<<"Data size:"<在服务器端通过接收到的二进制串便可以成功还原出数据：
JointTrajectoryPoints jtPts;
if(!jtPts.ParseFromArray(data, size))
{
cerr<<"Deserialization failed."<基于asio的通信
原来在linux下进行TCP的网络开发，都是直接使用的原始的socket方法进行编程，每次都觉得不够方便。最近看到boost::asio，就决定尝试一下，虽然开始也有一些小坑，但一天的功夫就搭好了基于tcp的C/S程序代码。其实都挺简单的，就直接贴源代码出来。
客户端的代码都在asio_client.h文件中，这里面只实现了最基本的两个操作，即连接服务器和发送数据。代码内容如下：
#include
using namespace std;
typedef boost::asio::io_service IoService;
typedef boost::asio::ip::tcp TCP;
class asio_client: public boost::enable_shared_from_this
{
public:
asio_client(): socket_(ios_)
{
ios_.run();
}
bool connect(string srvip, int port) {
TCP::endpoint ep(boost::asio::ip::address::from_string(srvip), port);
socket_.connect(ep);
cout<<"connected."< 服务器端的代码就稍微复杂一些，涉及到一些异步操作。一些小坑和注意事项都写在注释中。其具体代码如下：
#include
#include
#include
#include
#include
#include
#define BUFFER_SIZE 64*1024
#define TIMEOUT 10
using namespace std;
typedef boost::asio::io_service IoService;
typedef boost::asio::ip::tcp TCP;
//这是一个基类，当获得数据完成后会调用其Callback函数。继承该类用于任务相关处理
class TcpDataHandler{
public:
TcpDataHandler(){}
virtual ~TcpDataHandler(){}
virtual void Callback(char* data, int size)=0;
};
//该类用于处理每一个tcp连接，连接关闭时自动释放
class tcp_connection
:public boost::enable_shared_from_this
{
public:
typedef boost::shared_ptr pointer;
static pointer create(IoService &io_service, TcpDataHandler *dataHandler) {
return pointer(new tcp_connection(io_service, dataHandler));
}
TCP::socket &socket() {
return socket_;
}
// 已经确立连接，接收数据
void start()
{
// 注册一个用于接收的callback, 首先接收数据的长度信息
boost::asio::async_read(socket_, boost::asio::buffer(msg_,sizeof(int)),
boost::bind(&tcp_connection::read_handle, shared_from_this(), boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred, false) );
}
private:
tcp_connection(IoService &io_service, TcpDataHandler* dataHandler) :socket_(io_service),timer_(io_service),
dataHandler_(dataHandler) { }
//定时器的回调函数
void timeout_handle(const boost::system::error_code & ec)
{
//这一行判断很重要，因为不光超时，操作中断也会调用此函数
if(ec==boost::system::errc::operation_canceled) return;
cerr<<"receive data time out"<BUFFER_SIZE) {
cerr<<"buffer is not enough to hold the data."<Callback(msg_, bytes_transferred);
//中止掉定时器
timer_.cancel();
}
}
private:
TCP::socket socket_;
boost::asio::deadline_timer timer_;
TcpDataHandler *dataHandler_;
char msg_[BUFFER_SIZE];
};
class asio_tcp_server
{
public:
asio_tcp_server(int port, TcpDataHandler *handler ):
acceptor_(ios, TCP::endpoint(TCP::v4(), port) )
{
dataHandler_=handler;
}
//该函数首先创建一个tcp_connection类，然后侦听来自客户端的连接
void start_accept()
{
tcp_connection::pointer new_connection =
tcp_connection::create(acceptor_.get_io_service(), dataHandler_);
acceptor_.async_accept(new_connection->socket(),
boost::bind(&asio_tcp_server::handle_accept, this, new_connection,
boost::asio::placeholders::error) ) ;
ios.run();
}
private:
//当accept一个连接后便交由tcp_connection去处理，然后继续保持侦听
void handle_accept(tcp_connection::pointer new_connection, const boost::system::error_code& error)
{
if(!error){
new_connection->start(); //do some read and write work.
}
start_accept();
}
private:
IoService ios;
TCP::acceptor acceptor_;
TcpDataHandler *dataHandler_;
};
脉冲发送
到这里我们就可以在树莓派上接收到规划好的轨迹点数据了。接下来便是让树莓派产生适当的脉冲让步进电机或者伺服电机转动起来了。为了通过树莓派产生精确的脉冲，特意设计封装了一个简单易用的脉冲发生器，其设计在另一个文档中进行介绍。
我们这里只需要派生TcpDataHandler类，实现其中的Callback函数即可。在该函数中首先进行反序列化，得到JointTrajectoryPoints对象，然后就依据其中的各个轨迹点之间的位置变化和时间变化，确定在时间段内要发送脉冲数量。我们这里仅以关节1作为范例，具体的代码为：
void Callback(char* data, int size)
{
if(!jtPts.ParseFromArray(data, size) ) {
cerr<<"Deserialization error."<init(1000);
google::protobuf::RepeatedPtrField::iterator prev_pt_iter, cur_pt_iter;
prev_pt_iter=jtPts.points().begin();
for(cur_pt_iter=prev_pt_iter+1; cur_pt_iter!=jtPts.points().end(); prev_pt_iter=cur_pt_iter, cur_pt_iter++ )
{
//计算时间段的时间
tmSpendInUs=(int)((cur_pt_iter->time_frome_start()-prev_pt_iter->time_frome_start())*1000*1000);
//calculate how many pulses need to send. do unit conversion
//将以弧度表示的位置转换为所需发送的脉冲数，其中1.8*2为细分的单个脉冲角度，36为减速器倍数。
cmdPosInPulses=(int)(cur_pt_iter->positions(0)*(360.0/3.1415926)/(1.8*2)* 36);
pulses_needed=cmdPosInPulses-curPosInPulses;
cout<<"pulses: "<setParam(0,AxisMode::AXISMODE_PULSE_WITh_FIXED_WIDTH, ¶m, 1);
rpiAxis->setCmd(0,pulses_needed);
rpiAxis->startCmd();
while(!rpiAxis->isFinished()) ; //等待发送结束
//更新位置
curPosInPulses =cmdPosInPulses;
}
}
将树派上的程序命名为ros_motor_control。为了测试再将整个系统都运行起来：
### Raspberry Terminal
$ ./ros_motor_control
### PC Terminal 1
$ rosrun trajectory_test trajectory_test_node
### PC Terminal 2
$ roslaunch panda_moveit_config demo.launch
### PC Terminal 3
$ roslaunch moveit_tutorials move_group_interface_tutorial.launch
这样，当通过RViz中的"Next"触发了move_group节点的轨迹规矩后，move_group将其通过Action发给trajectory_test_node（里面运行Action Server），trajectory_test_node再通过TCP+protobuf组合技术发送给树莓派上的ros_motor_control，ros_motor_control就让电机按照具有加减速的轨迹运动起来了。