razor imu 9dof的使用，先锋机器人rosaria 理解,配置STM32-ROS通信等疑难杂症（持续更新中）

　　　　$\color{blue}{\bf本文由}$ $\color{red}{\it @David Han}$$\color{blue}{ \bf出品 ,转载请注明出处}$
　　　　$\color{green}{\bf文章链接：}$http://blog.csdn.net/David_Han008/article/details/77850460

　　　　
遇到的问题1：razor_imu_9_dof launch文件启动报错：
分析原因：
在源码的Output.ino当中，是这么写的

但是在启动的python文件当中，却需要删减去字符#YPRAMG因此出现的启动机器人的时候，没有办法删除字符。我尝试过在python文件中，直接将删除的字符改成#YPR，但是在后面的部分需要更改的太多，因此放弃了方式。不如从原始的数据当中，直接将AMG的数据添加进去。将这部分修改为：

上面修改后的版本直接可以供我们github上面下载到，下载地址；
下载下来，后面就可知直接使用了。

2017.11.26更新和补充：
在后面的使用中，博主发现这个IMU的数值总是不准确。因此需要对加速度计、陀螺仪和磁力计的数值进行校正。
1.1 校正磁力计
首先，需要确认，已经在razor imu 9dof中下载好博主对应的代码了。这样，接下来，在win下，下载：processing，然后在下载EJML 0.17 or 0.23.jar（在后面，我会对应的软件放在腾讯微云当中）。
这个processing软件下载还之后，点击运行一下，然后在文档那个目录中产生一个Processing文件夹，这个文件夹用来存放我们EJML.jar
接下来：

吧博主github上面代码下载下来。然后点击运行Magnetometer_calibration.pde，
可能遇到的问题，出现如下报错：（当然最好是没有这个问题，博主笔记本显卡有问题，就遇到了）

解决办法：

将上图中的两个文件直接删除，然后再运行Magnetometer_calibration.pde的时候，一直点确定，就OK了，接下来就见到这个界面，然后点击运行按钮

会提示有报错，说EJML没有，接下俩我们把博主提供的EJML放到里面（如果没有对应文件夹就新建）

然后退出来，重新启动，然后再点击运行按钮， 尽量整个胳膊挥动IMU，然后会出现有颜色的点，
时间有限，我就先放在在一张了。

然后点击空格，就可以将对应额校正文件存下来，然后会生成一个.float文件

这个文件，是用来画matlab图像的，将个文件加到matlab的当前目录中

然后在点击运行就OK了，（补充一下，这个matlab文件在）

最后的结果：

最后我们需要往arduino里面修改额参数

#define CALIBRATION__MAGN_USE_EXTENDED true
const float magn_ellipsoid_center[3] = {-68.6775, -371.564, 198.617};
const float magn_ellipsoid_transform[3][3] = {{0.840899, 0.00174218, 0.0233947}, {0.00174218, 0.857355, 0.0608349}, {0.0233947, 0.0608349, 0.970393}};

软件的下载链接：点击这里

2、校正加速度计
3、校正陀螺仪

razor 规定的xyz的方向

ros当中，规定的xyz的方向

当你发送的cmd_vel是正值的时候，小车向Y轴方向转动

然后，当我们在做小车的时候，一定要注意轮子间距等问题。这个问题比较隐蔽。就是使用差动轮的运动模型，将当前的速度转化成左右轮的转速。

---

问题2：ROS当中将IMU发布的四元数转化成欧拉角，提取出欧拉角
在这个imu当中，ROS的imu转向的数据是通过四元数的形式发布的的，因此，如果我接收的值需要一个yaw值的话，需要从四元数转到欧拉角。提取出当前的yaw值，然后将yaw值进行赋值。
ROS当中将IMU发布的四元数转化成欧拉角，提取出欧拉角的教程：点击这里，我也会吧核心的代码写到这里。

  tf::Quaternion RQ2;
  double roll,pitch,yaw;
  position.pose.pose.orientation.x=imu.orientation.x;
  position.pose.pose.orientation.y=imu.orientation.y;
  position.pose.pose.orientation.z=imu.orientation.z;
  position.pose.pose.orientation.w=imu.orientation.w;
tf::quaternionMsgToTF(position.pose.pose.orientation,RQ2);
  tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw);
  yaw=angles::normalize_angle_positive(yaw);

tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(yaw), tf::Vector3(pos.getX()/1000,
  pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.

问题3，ROS当中如何订阅razor_imu_9dof发布的imu的数据。
首先启动这个imu之后，参考effect Robotic programming with ROS这本书。

#include <sensor_msgs/Imu.h>
sensor_msgs::Imu imu;
void imuCallback(const sensor_msgs::Imu &imu_msg)
{
imu = imu_msg;
}
ros::Subscriber imu_sub = n.subscribe("imu_data", 10, imuCallback);
odom_trans.transform.rotation.x = imu.orientation.x;
odom_trans.transform.rotation.y = imu.orientation.y;
odom_trans.transform.rotation.z = imu.orientation.z;
odom_trans.transform.rotation.w = imu.orientation.w;
odom.pose.pose.orientation.x = imu.orientation.x;
odom.pose.pose.orientation.y = imu.orientation.y;
odom.pose.pose.orientation.z = imu.orientation.z;
odom.pose.pose.orientation.w = imu.orientation.w;

部分的核心代码就是这里，当然，你需要利用odom_trans.transform来发布TF变换。利用odom.pose.pose.orientation.x 来发布里程计信息;

我这里吧effecive这本书的代码贴到这里，方便大家分析学习

 #include <string>
 #include <ros/ros.h>
 #include <sensor_msgs/JointState.h>
 #include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
 #include <std_msgs/Float32.h>
 #include <sensor_msgs/Imu.h>

double width_robot = 0.1;
double wheel_radius = 0.025;
double vx = 0;
double vy = 0;
double vth = 0;
sensor_msgs::Imu imu;

void imuCallback(const sensor_msgs::Imu &imu_msg)
{
    imu = imu_msg;
}

void cmd_velCallback(const geometry_msgs::Twist &twist_aux)
{
    geometry_msgs::Twist twist = twist_aux; 
    double vel_x = twist_aux.linear.x;
    double vel_th = twist_aux.angular.z;
    double right_vel = 0.0;
    double left_vel = 0.0;

    left_vel = (twist_aux.linear.x - width_robot*twist_aux.angular.z)/wheel_radius;
    right_vel = (twist_aux.linear.x + width_robot*twist_aux.angular.z)/wheel_radius;
    vx = left_vel;
    vy = right_vel;
    vth = twist_aux.angular.z;
}

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

    ros::init(argc, argv, "odom");
    ros::NodeHandle n;
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);
    ros::Subscriber cmd_vel_sub = n.subscribe("sensor_velocity", 10, cmd_velCallback);
    ros::Subscriber imu_sub = n.subscribe("imu", 10, imuCallback);

    // initial position
    double x = 0.0; 
    double y = 0.0;
    double th = 0;

    ros::Time current_time;
    ros::Time last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(20);

    const double degree = M_PI/180;

    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    while (ros::ok()) {
        current_time = ros::Time::now(); 

        double dt = (current_time - last_time).toSec();
        double delta_x = wheel_radius*(vx +vy)* cos(th)* dt/2;
        double delta_y = wheel_radius*(vx+vy)*sin(th) * dt/2;
        double delta_th = vth * dt;

        x += delta_x;
        y += delta_y;
        th += delta_th;

        geometry_msgs::Quaternion odom_quat;    
        odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th);

        // update transform
        odom_trans.header.stamp = current_time; 
        odom_trans.transform.translation.x = x; 
        odom_trans.transform.translation.y = y; 
        odom_trans.transform.translation.z = 0.0;
        //odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th);
        odom_trans.transform.rotation.x = imu.orientation.x;
        odom_trans.transform.rotation.y = imu.orientation.y;
        odom_trans.transform.rotation.z = imu.orientation.z;
        odom_trans.transform.rotation.w = imu.orientation.w;
        //filling the odometry
        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_link";

        // position
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = 0.0;
        //odom.pose.pose.orientation = odom_quat;
        odom.pose.pose.orientation.x = imu.orientation.x;
        odom.pose.pose.orientation.y = imu.orientation.y;
        odom.pose.pose.orientation.z = imu.orientation.z;
        odom.pose.pose.orientation.w = imu.orientation.w;

        odom.pose.covariance[0] = (1e-3);
        odom.pose.covariance[7] = (1e-3);
        odom.pose.covariance[14] = (1e-6);
        odom.pose.covariance[21] = (1e-6);
        odom.pose.covariance[28] = (1e-6);
        odom.pose.covariance[35] = (1e-3);
        //velocity
        odom.twist.twist.linear.x = wheel_radius*(vx+vy)/2;
        odom.twist.twist.linear.y = 0;
        odom.twist.twist.linear.z = 0.0;
        odom.twist.twist.angular.x = 0.0;
        odom.twist.twist.angular.y = 0.0;
        odom.twist.twist.angular.z = vth;
        odom.twist.covariance[0] = 1e-3;
        odom.twist.covariance[7] = 1e-3;
        odom.twist.covariance[14] = 1e-3;
        odom.twist.covariance[21] = 1e-3;
        odom.twist.covariance[35] = 1e-3;
        last_time = current_time;

        // publishing the odometry and the new tf
        broadcaster.sendTransform(odom_trans);
        odom_pub.publish(odom);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

然后下面我吧rosaria当中修改后的代码贴出来，其实只要这套发布的机制搞清楚就可以了。

void RosAriaNode::publish()
{
  // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
  pos = robot->getPose();

   // tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000,
   // pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.\
    //定义一个四元数用来接受变换



   tf::Quaternion RQ2;


  double roll,pitch,yaw;
  position.pose.pose.orientation.x=imu.orientation.x;
  position.pose.pose.orientation.y=imu.orientation.y;
  position.pose.pose.orientation.z=imu.orientation.z;
  position.pose.pose.orientation.w=imu.orientation.w;


  tf::quaternionMsgToTF(position.pose.pose.orientation,RQ2);
  tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw);
  yaw=angles::normalize_angle_positive(yaw);



  tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(yaw), tf::Vector3(pos.getX()/1000,
  pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.\

   position.twist.twist.linear.x = robot->getVel()/1000.0; //Aria returns velocity in mm/s.
   position.twist.twist.linear.y = robot->getLatVel()/1000.0;
   position.twist.twist.angular.z = robot->getRotVel()*M_PI/180;



  position.header.frame_id = frame_id_odom;
  position.child_frame_id = frame_id_base_link;
  position.header.stamp = ros::Time::now();
  pose_pub.publish(position);

/*
  ROS_INFO("RosAria: publish: (time %f) : pos.getTh():%f, yaw: %f", 
    position.header.stamp.toSec(), 
    (double)pos.getTh()*M_PI/180,yaw
  );


*/


  ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, pose y: %f, pose angle: %f; linear vel x: %f, vel y: %f; angular vel z: %f", 
    position.header.stamp.toSec(), 
    (double)position.pose.pose.position.x,
    (double)position.pose.pose.position.y,
    (double)position.pose.pose.orientation.w,
    (double)position.twist.twist.linear.x,
    (double)position.twist.twist.linear.y,
    (double)position.twist.twist.angular.z
  );

  // publishing transform odom->base_link 这部分是TF比那换
  odom_trans.header.stamp = ros::Time::now();
  odom_trans.header.frame_id = frame_id_odom;
  odom_trans.child_frame_id = frame_id_base_link;

  odom_trans.transform.translation.x = pos.getX()/1000;
  odom_trans.transform.translation.y = pos.getY()/1000;
  odom_trans.transform.translation.z = 0.0;
  odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(yaw);

  //如果吧这段去掉，导致的TF的数据是o
/*
  odom_trans.transform.rotation.x = imu.orientation.x;
  odom_trans.transform.rotation.y = imu.orientation.y;
  odom_trans.transform.rotation.z = imu.orientation.z;
  odom_trans.transform.rotation.w = imu.orientation.w;
  */
  odom_broadcaster.sendTransform(odom_trans);

问题4：在面向对象编程的过程当中，订阅话题需要加取地址符号和this指针

我们在面向过程编程的时候，代码是这样写的：

 ros::Subscriber cmd_vel_sub = n.subscribe("sensor_velocity", 10, cmd_velCallback);
void imuCallback(const sensor_msgs::Imu &imu_msg)
{
    imu = imu_msg;
}

但是在面向对象编程当中，我们应该这么写：

imu_sub=n.subscribe("/imu", 10, &RosAriaNode::imuCallback, this);

就是函数的名称，然后加上this指针。

问题5，先锋机器人使用编码器测量转角，
我现在的想法，利用编码器测量转向，然后使用扩展卡尔曼滤波滤波的节点，融合imu和编码器的数据。将输出的数据提供给机器人。
在先锋机器嗯当中，转角的pos.getTh()*M_PI/180这个yaw值，编码器和陀螺仪经过卡尔曼滤波的到yaw值,，如果你想要经过编码器得到yaw值，应该改成robot->getEncoderTh()*M_PI/180这样就好了
详细的资料，我已经在rosaria的github上提交，跳转链接：点击这里

问题6，在win下安装keil,吧程序烧如到stm32开发板当中，然后通过usb口，将stm32和ubunt系统中的ROS进行通信

前言说明：
硬件：STM32F103ZET6或者STM32F407VET6、ST-LINK2和USB转TTL
软件：keil uVision4和ST-LINK2、ROS_indigo版本（ROS的版本和roslib关系不大）

• 6.1keil的安装 点击这个网页，以及ST_LINK驱动的安装，点击这个网页。尽快保存，里面有破解版的keil uVision4（MDK4.12）

• 6.2 STM32工程的配置和相关引脚的配置
  我是参考的 博文，博文对应的代码。下载好之后，相信你已经安装好keil uVision4和STM_LINK的驱动了。
  下载好博主的代码之后，双击打开ros_test.uvproj
  
  点击魔术棒，进行配置工程
  
  选择你target的芯片（根据你情况而定）
  
  选择晶振赫兹，默认8-30MHz都可以
  
  选择Debug下的ST-Link Debugger
  
  在utilities下模式下选择ST-Link Debugger模式，并且点击setting
  
  点击add,然后哎下拉列表中找到你需要选择的芯片的型号。尽量选个大点的。最后点击确定，配置完成
  
  最后编译和下载到板子上面
  

接下来是关于接线引脚的问题，在源码的usart3.c文件中，可以看出pin_9是TX pin_10是RX，那么对应你的USB转TTL的部分插上对应的引脚。

• 6.3 roslib加入后ros代码的解读

 #include "includes.h"
 #include "std_msgs/String.h"

void msgCallBack(const std_msgs::String& msg);
std_msgs::String msg1;

//-------------ROS±äÁ¿----------------
ros::NodeHandle nh;
ros::Subscriber<std_msgs::String> sub("stm_subscribe", msgCallBack);
ros::Publisher pub("stm_publish",&msg1);
int i=0;


void msgCallBack(const std_msgs::String& msg)
{   
    char hello[13];
    sprintf(hello, "%d", i++);    
    msg1.data = hello;
    pub.publish(&msg1);
    i++;
}

int main(void)
{   

    System_Init();
    delay_ms(1000);
    nh.initNode();
    nh.advertise(pub);
    nh.subscribe(sub);
    char hello[13] = "hello world!";
    while(1)
    {
        // msg.data = hello;
        // pub.publish(&msg);
        nh.spinOnce();
        delay_ms(10);
    }

• 6.4 STM32F103通信效果
  电脑端通信效果
  
  我的USB转TTL是绿灯是RX 红灯是TX
  

  6.5 将来关于串口部分可能会封装到一个节点当中
  相关的参考资料，点击这个链接

  常见报错：
  

Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino

---

出现这个问题很可以是的接线的问题，检查一下你的rx和tx接线是不是正常。

最后感谢 stoneyang的大力帮助和指导，以及他在原来博主STM405版本基础上改进的STMF103的版本，如果是103的朋友，可以直接去他的github上面点击下载：链接

问题7，rosserial消息发布机制
和学长一起探索的rosseerial的发布机制，需要搞清楚的是rosserial到底给STM32发布那些指令。这些指令能不能被解析出来，我们使用的是ubuntu内置的usb_mon，参看连接：点击这里
这里和上面略有不同，sudo modprobe usbmon进行安装。
我是Ubuntu上面进行测试，那么这里正确是路径应该是/sys/kernel/debug/usb/devices,前提是你需要sudo su 之后。

问题8，STM32底盘搭建日常小节
最近这段时间接触了不少STM32相关的硬件方面的知识，我想吧他们记录在自己的博客当中，以视对这些知识的尊重。
1、RS232串口线

这是RS232的串口线，其中最重要的口就是上排的2口，3口和5口。2口是RX ，3口是TX，5口是GND。通过两根RS232的串口线，可以实现两个笔记本之间进行通信。举个例子，就是Ubuntu打开串口，发送数据到win，win打开串口数据接收数据。
2、拨线器

这是用来拨线的，将一根线顶到上面哪个红色的口的地方（如果没有顶到，拨出来的线会不够长，导致后面的给线安装口的时候，安装的口不能用），然后一捏就OK了。
3、捏口器

首先，找个金属扣，就是下面的这个金属扣，然后从下面塞进去，然后捏紧钳子，用力，然后将线从另外的口的一端插进去
金属扣就是下面的这个样子。

然后STM32的板子，是我们自己画的板子，电路图是有出入的，就是将32 33 和24电阻拿掉，就可以正常工作了。

在做线的时候，口朝上，然后塞入到白色的口当中

焊芯片的时候，电烙铁的温度控制在320度最好

橡胶套烘烤器

问题9，使用QRcode进行二次定位
参考github的地址：点击这里
前提：需要安装zbar,参考博客：点击这里
上面的博客当中，并没有提供下载的链接，所以我在这里吧现在的对应软件包下载链接提供了：
libjpeg9下载链接：点击这里
ImageMagick-6.9.5-7.tar.gz（这个我用的6.9.5.10实测通过）下载链接：点击这里
zbar-0.10.tar下载链接：点击这里
问题10，使用联合体将float和unsigned char 类型进行转换
在串口通信部分，我们给底盘控制器速度cmd_vel的数值，这个数据值一个float类型，但是在串口通信部分，我们发布的unsigned char类型给串口，对于各种各样的速度的数值，我们需要一个转换的方法。联合体（union）提供了这样的方式，通过共享内存的方式，将unsigned char类型和float类型进行互换。
参考以下代码：左轮4个字节，右轮4个字节，最后1个字节是结束位。总共9个字节。

//以下为串口通讯需要的头文件
 #include <string>        
 #include <iostream>
 #include <cstdio>
 #include <unistd.h>
 #include <math.h>
 #include "serial/serial.h"
/****************************************************************************/
using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;
/*****************************************************************************/
 #define PI 3.14159
float ratio = 1000.0f ;   //转速转换比例，执行速度调整比例
float D = 0.576f ;    //两轮间距，单位是m
float encoder_ppr = 1024;
float wheel_radius = 0.125;
float linear_temp=0,angular_temp=0;//暂存的线速度和角速度
/****************************************************/
unsigned char data_terminal0=0x3e;  //“/r"字符
unsigned char speed_data[9]={0};   //要发给串口的数据
//发送给下位机的左右轮速度，里程计的坐标和方向
union floatData //union的作用为实现char数组和float之间的转换
{
    float d;
    unsigned char data[4];
}right_speed_data,left_speed_data,position_x,position_y,oriention,vel_linear,vel_angular;
/************************************************************/
void callback(const geometry_msgs::Twist & cmd_input)//订阅/cmd_vel主题回调函数
{
    angular_temp = cmd_input.angular.z ;//获取/cmd_vel的角速度,rad/s
    std::cout<<"cmd_vel_angular.z:"<<cmd_input.angular.z<<std::endl;
    linear_temp  = cmd_input.linear.x ;//获取/cmd_vel的线速度.m/s
    std::cout<<"cmd_vel_angular.x:"<<cmd_input.linear.x<<std::endl;
    //将转换好的小车速度分量为左右轮速度
    left_speed_data.d  = linear_temp - 0.5f*angular_temp*D ;
    right_speed_data.d = linear_temp + 0.5f*angular_temp*D ;
    //存入数据到要发布的左右轮速度消息
    left_speed_data.d*=ratio;   //放大１０００倍，mm/s
    right_speed_data.d*=ratio;//放大１０００倍，mm/s

    for(int i=0;i<4;i++)    //将左右轮速度存入数组中发送给串口
    {
        speed_data[i]=right_speed_data.data[i];
        speed_data[i+4]=left_speed_data.data[i];
    }
    //在写入串口的左右轮速度数据后加入”结束位“
    speed_data[8]=data_terminal0;
for(int i=0;i<9;++i)
{
std::cout<<std::hex<<(int)speed_data[i]<<std::endl;
}
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "base_controller");//初始化串口节点
    ros::NodeHandle n;  //定义节点进程句柄
    ros::Subscriber sub = n.subscribe("cmd_vel", 50, callback); //订阅/cmd_vel主题
    ros::spin();
    return 0;
}

这部分要根据自己单片机的代码书写，发送给单片机的程序，然后上面的部分，
写入串口的数据：

     serial::Serial my_serial;
     my_serial.setPort("/dev/ttyUSB0");
     my_serial.setBaudrate(115200);
     serial::Timeout       to=serial::Timeout::simpleTimeout(1000);
     my_serial.setTimeout(to);
     my_serial.open();
    通过。。write来向串口数学数据
    my_serial.write(speed_data,14);

问题10扩展，ros和STM32通信问题-串口读写

在这个坑上面，我大概走了2周，现在将这两周的经验总结如下：
首先非常感谢wjwwood，开发的serial包，下载链接：点击这里，多亏了wjwwood，我们可以在cmakelist.txt当中，可以添加serial这个包，

find_package(catkin REQUIRED COMPONENTS
  roscpp
  serial
)

然后在cpp文件当中，加入头文件`#include”serial/serial.h”`后，就可以很方便的使用serial当中的读写串口指令，（我接触了一天半的串口读写的问题，下面的话，可能不是很专业：使用这个serial包之后，完全不用考虑 同步读写，异步读写，互斥锁，进行线程之类，让我这种CPP新手，也可以实现串口读写的功能，简直大爱。） 参考模板：

 #include <ros/ros.h>
 #include <serial/serial.h>
 #include <std_msgs/String.h>
 #include <std_msgs/Empty.h>
serial::Serial ser;
void write_callback(const std_msgs::String::ConstPtr& msg){
    ROS_INFO_STREAM("Writing to serial port" << msg->data);
    ser.write(msg->data);
}
int main (int argc, char** argv){
    ros::init(argc, argv, "serial_example_node");
    ros::NodeHandle nh;
    ros::Subscriber write_sub = nh.subscribe("write", 10, write_callback);
    ros::Publisher read_pub = nh.advertise<std_msgs::String>("read", 2);
    try
    {
        ser.setPort("/dev/ttyUSB0");
        ser.setBaudrate(9600);
        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::Rate loop_rate(5);
    while(ros::ok()){
        loop_rate.sleep();
        if(ser.available()){
            //ROS_INFO_STREAM("Reading from serial port");
            std_msgs::String result;
            result.data = ser.read(ser.available());
            //ROS_INFO_STREAM("Read: " << result.data);
            ROS_INFO("%s", result.data.c_str());
            read_pub.publish(result);
        }
        ros::spinOnce();
    }
}

接下来，对上面的代码进行分析：如何向串口读写呢？首先我们要实例化一个serial 类的对象ser, 然后打开串口，设置好波特率等等，然后用 ser.write(data,长度)来向串口写入数据，长度就是你的数组的长度，data就是你要的数组。读串口，就是在while循环当中，ser.read(21)，这里的read有3个重载的函数，详细可以看wjwwood的手册。
上面的这种形式，适用于打开串口，什么都不用向单片机发送，单片机主动给你发送数据，但是实际情况是，我们的单片机是问答模式，简单来说，就是你需要想单片机发送一条特殊的指令，单片机才会想你发送数据。然而，在你发布速度的时候，因为发送速度，转化成左右轮的速度后，还是会产生一条16进制的指令，发送给单片机，因此我担心的是，如果这两条指令发送冲突了怎么办？起初，我联想到了，互斥锁，定时器等等，但是真的太难了，一下子搞不定。多亏了wjwwood的serial包足够的完善，在原来的代码上面加入了if(ser.available())就解决了这个问题。当然，在这条命令之前，你还是要发送那条问答模式下的接收命令的指令。

还想补充一下：上面代码中ros::spinOnce();如果没有这句话，回调函数不生效，ros::spinOnce();在while循环中使用，ros::spin()不再while循环中使用。另外loop_rate.sleep();是while循环的频率。还有cos()函数括号内的单位是弧度。

后面我会吧我的代码上传到我的github上面，大家敬请期待！
附录：
期间还参考了steven的博客：链接
以及它的github:链接

---

9dof razor论文必备补充资料：
1、硬件组成：
9DOF Razor IMU包含4个传感器：一个LY530AL（单轴陀螺仪），LPR530AL（双轴陀螺仪），ADXL345（三轴加速计），和HMC5883（三轴磁力计）。为了给你9自由度的惯性测量。所有传感器的输出由板上的ATmega328处理通过串行接口输出。
2、历史简介：
通过Jordi Munoz 和其他人的工作，该 9DOF Razor 能够成为参考系的标杆。这使得9DOF Razor成为一个非常强大的UAVs，自备交通工具和图像稳定化的控制机构。
3、使用指南：
该板通过8MHZ Arduino bootloader 和样例固件编写程序，测试所有传感器的输出。
可以很方便地将串行的TX和RX脚连接到一块3.3V FTDI Basic Breakout或USB Serial light适配器,将终端程序以57600bps打开，随后菜单会通过测试传感器来指导你。你可以使用Arduino IDE编写你的代码到9DOF，只要选择“Arduino Pro or Pro Mini (3.3v, 8mhz) w/ATmega328”即可。
该9DOF工作电压为3.3V；任何供电电源连接到白色JST接口将会被控制到工作电压。输出端设计成了与3.3V FTDIBasic Breakout板匹配，这样你就可以很方便地将板子和电脑的USB端口连接，或者通过蓝牙或XBee进行无线传输。

---

电机的常识
1、步进电机
步进电机也叫做脉冲电机，是基于最基本的电磁铁原理，
步进电机：是将电脉冲信号转变为角位移或者线位移的开环控制电机。步进电机是一种感应电机，他的工作原理是 将直流电变成分时供电的多相时序控制电流，用这种多相的电流为步进电机进行供电，步进电机才能正常工作。（这就是电机驱动器干的活。）驱动器，为步进电机提供分时供电的，多相时序控制器
步进电机优点：
1、 电机的转的角度正比于脉冲数
2、 由于每步的精度在3%~5%之间，并且不会将上一步的误差累计到下一步。因而具有良好的位置精度和运动的重复性。
3、 由于没有电刷，具有可靠性高，因此电机的寿命仅仅取决于轴承的寿命
缺点：
1、 难以运动到较高的转速
2、 高速运转的时候，会发出震动和噪声
2、直流无刷电机
直流无刷电机
无刷电机的优点：
1、 利用电子换向代替的传统的机械换向，性能可靠，永无磨损，故障率第，寿命比有刷电机高6呗
2、 属于静态电机，空载电流小
3、 效率高，体积小
缺点
1、 低速情况下、有轻微的震动
2、 价格高
有刷电机优点：
1、 变速平稳，几乎感受不到震动
2、 温升低，可靠性高
3、 价格低
缺点：
1、 碳刷易磨损，更换较为麻烦，寿命短
2、 运行电流大
3、伺服电机
伺服电机再控制速度和位置精度方面，都非常精确，可以将电压信号转化为转矩和转速来驱动控制对象，伺服电机主要是靠脉冲进行定位的，基本上可以理解为，伺服电机接收一个脉冲弄，就会转一个脉冲的对应的角度，从而实现位移。交流伺服电机也是无刷电机。
步进电机和伺服电机最大的区别在于：步进电机是开环控制，而伺服电机是闭环控制的。
我们的选择
最后我们选择伺服闭环步进电机：57高速闭环步进电机 2.2N伺服闭环步进电机HBS57
什么叫伺服闭环步进电机：

产品特点：
1、 低发热量（在静止的状态下，电流近乎为0，无发热）
2、 平滑（基于反馈编码器的矢量空间矢量电流控制算法和矢量平滑滤波技术，抑制低频共振的现象）
3、 高速响应（伺服控制系统提供了大力矩的输出，步进伺服电机速度可以达到600-2000RPM）
电压是建议24V-48V
细分：256细分
电流4A 力矩2.2N/M 轴径8mm
额定转速 1000转，空载转速2000转
RPM转每分
这个产品的特点：
1、 无丢步、定位准确
2、 内置加速控制，改善平稳特性。
3、 振动小、低速运行平稳

详细参数，优缺点：参考淘宝链接

---

关于STM32基础知识
在main.c中写主要的代码和各种子函数。然后stm32f10x_it.c中写各种中断需要执行的代码。
必用模块初始化函数定义

1、定义时钟初始化函数
void RCC_Configuration(void);   
2、定义中断管理初始化函数
void NVIC_Configuration(void);
3、定义管脚初始化函数
 void GPIO_Configuration(void); 
4、定义延迟函数
void Delay(vu32 nCount); 

在main函数当中我们只用初始化是时钟函数，中断管理函数和管脚函数就可以了。
GPIO初始化代码：

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 ;//管脚号 
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//输出速度 
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;//输入输出模式 
GPIO_Init(GPIOB, &GPIO_InitStructure); //初始化 

简单的延迟函数

void Delay(vu32 nCount) //简单延时函数 
{ for (; nCount != 0; nCount--);}//循环计数延时 

基本的串口通信

1、初始化定义
void USART_Configuration(void); //定义串口初始化函数
2、初始化函数调用
void USART_Configuration(void)  //串口初始化函数 
{ 
//串口参数初始化   
  USART_InitTypeDef USART_InitStructure; //串口设置恢复默认参数 
//初始化参数设置 
  USART_InitStructure.USART_BaudRate = 9600; //波特率9600 
  USART_InitStructure.USART_WordLength = USART_WordLength_8b; //字长8位 
  USART_InitStructure.USART_StopBits = USART_StopBits_1; //1位停止字节 
  USART_InitStructure.USART_Parity = USART_Parity_No; //无奇偶校验 
  USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无流控制 
  USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;//打开Rx接收和Tx发送功能 
  USART_Init(USART1, &USART_InitStructure); //初始化 
  USART_Cmd(USART1, ENABLE);  //启动串口 
} 
3、在RCC中打开相应的串口
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 , ENABLE); 
4、在GPIO里面设置相应的管脚串口
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;//管脚9 
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出 
GPIO_Init(GPIOA, &GPIO_InitStructure); //TX初始化 
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;//管脚10 
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入 
GPIO_Init(GPIOA, &GPIO_InitStructure); //RX初始化 
5、发送数据
USART_SendData(USART1, 数据);//发送一位数据 

NVIC串口中断的应用

NVIC_InitTypeDef NVIC_InitStructure;//中断默认参数 
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQChannel;//通道设置为串口1中断 
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;//中断占先等级0 
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;//中断响应优先级0 
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //打开中断 
NVIC_Init(&NVIC_InitStructure);//初始化 

PWM输出

1、void TIM_Configuration(void); //定义TIM初始化函数 
2、初始化函数定义
void TIM_Configuration(void)//TIM初始化函数 
{  
//TIM3初始化 
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;//定时器初始化结构 
  TIM_OCInitTypeDef  TIM_OCInitStructure;//通道输出初始化结构 
//TIM3通道初始化 
  TIM_TimeBaseStructure.TIM_Period = 0xFFFF;        //周期0～FFFF 
  TIM_TimeBaseStructure.TIM_Prescaler = 5;          //时钟分频 
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;      //时钟分割 
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;//模式 
  TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //基本初始化 
  TIM_ITConfig(TIM3, TIM_IT_CC4, ENABLE);//打开中断，中断需要这行代码  
//启动TIM3 
  TIM_OCStructInit(& TIM_OCInitStructure);//默认参数 
  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;    //工作状态 
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;//设定为输出，需要PWM输出才需要这行代码 
  TIM_OCInitStructure.TIM_Pulse = 0x2000;                                 //占空长度 
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;                 //高电平 
  TIM_OC4Init(TIM3, &TIM_OCInitStructure);                                 //通道初始化 
  TIM_Cmd(TIM3, ENABLE);                                                                        
} 
3、在RCC函数中加入TIM时钟开启
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM3, ENABLE); 
4、在GPIO中，将输入输出管脚模式进行设置
5、在终端中的NVIC中添加下面代码
//打开TIM2中断 
  NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQChannel;  //通道 
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;//占先级 
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;   //响应级 
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;     //启动 
  NVIC_Init(&NVIC_InitStructure);     //初始化 
6、改变占空比
TIM_SetCompare4(TIM3, 变量);