vrpn_client_ros通过rosserial将mocap创建的刚体信息发送给下位机

在roboware studio里添加cpp文件，内容如下，因为用到了rosserial，需要在cmakelists的第四行的括号内添加serial

#include  
#include   //ROS已经内置了的串口包 
#include  
#include  
#include  //发送小数

#include 
#include "vrpn_client_ros/vrpn_client_ros.h"
#include "tf/tf.h"
serial::Serial ser; //声明串口对象 
using namespace std;
using namespace vrpn_client_ros;
using namespace ros;  
//回调函数 
void write_callback(const geometry_msgs::PoseStamped::ConstPtr& infor) //定义收到的消息类型 几何位置时间戳变量infor
{ 
   // geometry_msgs/PoseStamped
  std::stringstream str;//定义一个数据流变量str
  std_msgs::String message;//定义一个字符变量，std_msgs 为ros里的消息机制
  //str<<"x="<positionpose_msg_.pose.position.x x<<"\r\n"
  //str<<"mcu receive"
  /***********以下设置四元数转欧拉角（注意需要添加tf的头文件）************/
  geometry_msgs::Quaternion orientation=infor->pose.orientation;//定义一个变量 oriention 类型为几何变量的四元数loat64 xfloat64 yfloat64 zfloat64 w
  tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));    
  double yaw, pitch, roll;    
  mat.getEulerYPR(yaw, pitch, roll);
  double pose_x,pose_y,pose_z;
  pose_x=infor->pose.position.x*1000;
  pose_y=infor->pose.position.y*1000;
  pose_z=infor->pose.position.z*1000;
   str<0?"+":"")<0?"+":"")<0?"+":"")<0?"+":"")<0?"+":"")<0?"+":"")<data); 
    ROS_INFO("serial got: [%s]", message.data.c_str());
  //  ser.write(msg->data);   //发送串口数据 
  // ROS_INFO_STREAM("Writing to serial port" );
   ser.write( message.data);

} 
  
int main (int argc, char** argv) 
{ 

  
    //订阅主题，并配置回调函数 
  //  ros::Subscriber write_sub = nh.subscribe("talker1", 1000, write_callback); 
    //发布主题 
   // ros::Publisher read_pub = nh.advertise("read", 1000); 
  
    try 
    { 
    //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyUSB0"); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 
  
    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    } 
      //初始化节点 
    ros::init(argc, argv, "output"); 
    //声明节点句柄 
    ros::NodeHandle nh; 
    //指定循环的频率 
   // ros::Rate loop_rate(50); 
  //  while(ros::ok()) 
  //  { 
  
   //     if(ser.available()){ 
   //         ROS_INFO_STREAM("Reading from serial port\n"); 
   //         std_msgs::String result; 
   //         result.data = ser.read(ser.available()); 
    //        ROS_INFO_STREAM("Read: " << result.data); 
    //        read_pub.publish(result); 
     //   } 
  //  ros::Subscriber write_sub = nh.subscribe("talker1", 1000, write_callback); 
     ros::Subscriber write_sub = nh.subscribe("/vrpn_client_node/RigidBody01/pose", 1000, write_callback);//前者为订阅的topic,1000为队列长度
        //处理ROS的信息，比如订阅消息,并调用回调函数 
        ros::spin(); 
       // loop_rate.sleep(); 
  return 0;
   // } 
} 

点击左上方的小锤子按钮，编译成功，当连接上optitrack，便可以向下位机打印当前的三维坐标信息，但实测发现，单片机性能较弱，运行一会儿，串口便会卡死，发现发送频率频率过快，默认100hz，虽然在launch文件里下可以修改，但发现没啥软用，具体的解决方法见下一篇博文。另外接收到的是刚体的位置信息与旋转信息，旋转信息是通过四元数来表示的，但通过转换的欧拉角与动捕软件上面显示的并不符合，研究了许久，未果！如果有知情者还请告知。

注意：需要有串口连接，才能运行！
linux下的串口调试助手：
sudo apt-get install picocom
sudo chmod 777 /dev/ttyUSB0
picocom -b 115200 /dev/ttyUSB0
按 Ctrl-a Ctrl-q 就可以退出终端。
dmesg | grep ttyS*显示插拔的串口