环境：Ubuntu16.04+ROS Kinetic

硬件：

Arduino mega2560（需拥有两个串口）

总线舵机（可采用dynamixel舵机，其带有ROS的功能包dynamixel_controllersTutorials）

前期准备：

1. 完成MoveIt!配置
   
2. 完成机器人在ROS平台的搭建

一、配置rosserial_arduino

1、安装环境

sudo apt-get install ros-kinetic-rosserial-arduino
sudo apt-get install ros-kinetic-rosserial

2、配置ros_lib（sketchbook为Arduino功能包位置）

  cd <sketchbook>/libraries
  rm -rf ros_lib
  rosrun rosserial_arduino make_libraries.py .

3、检查环境

提示：Window 环境的话，可以在Linux环境下配置好环境后将ros_lib文件夹提取到 Window 环境下并导入到Arduino

二、程序实现

发布器初始化方式

ros::Publisher _pub("/topic", &_msg);
nh.advertise(_pub);

订阅器初始化方式

void messageCb( const std_msgs::Int16MultiArray& _msg)
{}
ros::Subscriber<std_msgs::Int16MultiArray> sub("robot_hand_control", &messageCb );
nh.subscribe(sub); 

完整代码

#include <FlexiTimer2.h>
#include <SCServo.h>
#include <SCSProtocol.h>

#include <ros.h>
#include <ArduinoHardware.h>
#include <std_msgs/MultiArrayLayout.h>
#include <std_msgs/MultiArrayDimension.h>
#include <std_msgs/Int16MultiArray.h>

///////////////////////////////////ROS初始化/////////////////////////////////
ros::NodeHandle  nh;
 
///////////////////////////////////ROS订阅器/////////////////////////////////
int* Joint;
int MultiArray_length;
int count_length;
bool messageCb_judge,release_subCb_judge;

void messageCb( const std_msgs::Int16MultiArray& pos_msg)
{
   MultiArray_length=((pos_msg.data_length)/6);
   Joint[(pos_msg.data_length)];
   count_length=MultiArray_length;
   Joint=pos_msg.data;
   
  //start send angle 
  FlexiTimer2::set(650, Data_send);
  FlexiTimer2::start();
}
ros::Subscriber<std_msgs::Int16MultiArray> sub("robot_hand_control", &messageCb );

///////////////////////总线舵机控制/////////////////////////////////
SCServo smServo;
SCServo scsServo;

///////////////////////////////舵机参数//////////////////////////////
#define J1_Angle_Init 0
#define J1_Min_Angle_Limit 10
#define J1_Max_Angle_Limit 1013
#define J2_Angle_Init 0
#define J2_Min_Angle_Limit 10
#define J2_Max_Angle_Limit 4090
#define J3_Angle_Init 0
#define J3_Min_Angle_Limit 10
#define J3_Max_Angle_Limit 4090
#define J4_Angle_Init 0
#define J4_Min_Angle_Limit 10
#define J4_Max_Angle_Limit 4090
#define J5_Angle_Init 0
#define J5_Min_Angle_Limit 10
#define J5_Max_Angle_Limit 1013
#define J6_Angle_Init 0
#define J6_Min_Angle_Limit 10
#define J6_Max_Angle_Limit 1013

//////////////////////////////总线舵机中断函数/////////////////////////
int t = 600; 
int count = 0;
void Data_send()                    
{   
   if(count<count_length)
   {
    messageCb_judge = true;
   }
  else
   {
    count=0;
    MultiArray_length=0;
    FlexiTimer2::stop();
   }
}

///////////////////////主函数/////////////////////////////////
void setup() 
{  
///////////////////////ROS初始化/////////////////////////////////
  nh.initNode();
  nh.subscribe(sub); 

///////////////////////总线舵机初始化/////////////////////////////////  
  scsServo.End = 1;
  smServo.End = 0;
  Serial2.begin(1000000); 
  scsServo.pSerial = &Serial2;
  smServo.pSerial = &Serial2;
  delay(1000);
  messageCb_judge = false;
  
///////////////////////总线舵机角度初始化/////////////////////////////////  
  scsServo.EnableTorque(1, 1);
  smServo.EnableTorque(2, 1);
  smServo.EnableTorque(3, 1);
  smServo.EnableTorque(4, 1);
  scsServo.EnableTorque(5, 1);
  scsServo.EnableTorque(6, 1);
  scsServo.WritePos(1, J1_Angle_Init, 1000);
  smServo.WritePos(2, J2_Angle_Init, 1000);
  smServo.WritePos(3, J3_Angle_Init, 1000);
  smServo.WritePos(4, J4_Angle_Init, 1000);
  scsServo.WritePos(5, J5_Angle_Init, 1000);
  scsServo.WritePos(6, J6_Angle_Init, 1000);
}

void loop()
{ 
  if(messageCb_judge)
  {
    scsServo.WritePos(1,J1_Angle_Init- Joint[count*6+0], t);
    smServo.WritePos(2, J2_Angle_Init-Joint[count*6+1], t);
    smServo.WritePos(3, J3_Angle_Init+Joint[count*6+2], t);
    smServo.WritePos(4, J4_Angle_Init-Joint[count*6+3], t);
    scsServo.WritePos(5, J5_Angle_Init-Joint[count*6+4], t);
    scsServo.WritePos(6, J6_Angle_Init-Joint[count*6+5], t);
    play(1,Joint[count*6+0]);
    play(2,Joint[count*6+1]);
    play(3,Joint[count*6+2]);
    play(4,Joint[count*6+3]);
    play(5,Joint[count*6+4]);
    play(6,Joint[count*6+5]);
    count++;
    messageCb_judge = false;
   }
   nh.spinOnce();
}