基于STM32的rosserial_client的节点开发

写在最最最开头：先自我吐槽一下题目，写的跟毕设论文题目一样，希望大家不要在意，主要看内容_{_}

本文的主要目的就是介绍一下如何在stm32上开发ros了，或者说是移植arduino东西到ros上（顺便吐槽一下为什么ros给的demo不是我大stm32），先列出我主要参考的东西吧~

• rosserial的详细介绍：http://wiki.ros.org/rosserial
• rosserial_client的介绍：http://wiki.ros.org/rosserial_client
• rosserial_client的教程程：http://wiki.ros.org/rosserial_client/Tutorials
• rosserial协议的介绍：http://wiki.ros.org/rosserial/Overview/Protocol

想深入了解的同学记得要认真看一下哈~
当然，其实"移植"的最坏的结果就是解析ros协议了，在上面的第四个网址里面就可以看到关于协议的介绍，当然动手能力强的同僚也可以自己把ros的信息打印到串口之后自己解析，当然这样就特别麻烦了，不过身边确实有这么干的，效果也还不错，但是其中唯一的问题就是无法进行时间的同步（对于这种方法，笔者没有亲自试验过，按照身边同僚的试验说是无法进行，因此只能每次都上传geometry_msgs/Twist变量给PC上的一个节点，之后发布带时间的话题解决），所以对于ros中大多数话题的发布就比较麻烦了，例如tf、odom等带时间戳的话题。那么重点来了，如何不**“手动”**解析ros的协议就可以进行节点的收发呢（说白了就是像ros提供的arduino的例程一样）？
下面就是笔者在进行移植的心路历程：
首先给一个网址，是github上大神写好的stm32f1的驱动：
https://github.com/spiralray/stm32f1_rosserial
当时也是因为刚需，所以下载了下来，虽然这个工程不能在keil上打开，而且用的还是ST的C++的库，但是还是给了很大的启发，再次表示感谢！
其次就是移植了，下面是主要的步骤：

(1) 首先按照http://wiki.ros.org/rosserial_arduino/Tutorials上面的步骤配置好arduino IDE中的ros_lib，之后到~/sketchbook/libraries（默认目录）目录下复制ros_lib文件夹中的全部内容到keil下建立的工程中，聪明的你当然会把这个路径也加入到工程的配置中去，同时因为我们的工程要用ros的风格编程，记得在C/C++中的Misc Controls一栏写上–cpp，让keil支持C++编程（顺便一提，keil对C++的支持好像不是很到位，模板类好像一直编译不过，不过建议还是用C和C++的混编，因为说实话，C++在32上跑的确实不快）。

(2) 根据ros的教程，我们要写一个文件叫做stm32Hardware.h（建议保存在ros_lib中），其中要写一个类class stm32Hardware，其中包括5个函数，分别是构造函数（一般不写东西）、初始化函数（init()，一般写串口初始化）、读串口函数（read）、写串口函数（write(data,length)）、返回时间函数（unsigned long time()），下面主要说明读串口函数、写串口函数、返回时间函数。

(3) 返回时间函数，直接返回滴答定时器的数值就行。

(4) 读串口和写串口函数，这里读写串口就用到串口的最基本的读写啦，虽然stm32提供了很强大的串口空闲中断与DMA，但是在这里还是使用最基本的中断读写，但是并不同于笔者一般用的读写（笔者程序写的比较菜，一般写串口就是一个while里面加入send函数> <，读串口要么用DMA+空闲，要么一个一个读），这里需要采用一个软件模拟的读和写队列进行读写，下面重点讲解一下这个步骤：
<1>模拟一个队列，这里的队列其实就是一个数组啦（不要往数据结构上扯，那样太麻烦啦），之后用变量head，tail记录队列的头和尾，注意，head记录当前接收到什么位置了，而tail表示被读的位置，当head==tail的时候，就说明队列是满的了（要存在这里，但是这里的数据还没有被读取走）；
<2>串口接收中断，除了正常的接收外，我们要做的就是把读出的数值存入队列中了，其实就是在head处放入读出的数值，之后head自加，最后判断head是否等于tail，等于tail则关闭串口接收中断，具体代码为（顺便提一下，github上的代码如下所示，我并没有十分理解这样的方式，但是这样确实是好用的，但是如果你按照我给的方式进行操作的话，结果也是可以的）：

 if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
 {
      uint16_t tempRX_Head = (RX_Head + 1) & (UART_BUFSIZE-1);
      
      uint16_t tempRX_Tail = RX_Tail;
      uint8_t data = USART_ReceiveData(USART3);
      
      if (tempRX_Head == tempRX_Tail) {
          USART_ITConfig(USART3, USART_IT_RXNE, DISABLE);
      }else{
          RX[RX_Head] = data;
          RX_Head = tempRX_Head;
      }
  }

<3>接收队列的读取，我们接收到了很多数据，自然是要给用程序来进行处理的，不过按照这种移植的方式的话，我们就可以完全不用关心如何解析这些乱七八糟的数据，而更关心我们的应用层的编程。言归正传，我们要写两个函数，一个是判断当前队列是否是满的（也就是head！=tail），另一个就是拿到队列里面应该被读的数据（也就是先读出来tail处的值，之后tail自加），代码如下：

char Usart_DataAvailable()
{
    uint16_t tempHead = RX_Head;
    uint16_t tempTail = RX_Tail;    
    return (tempHead != tempTail);
}

uint8_t Usart_Getch()
{
    uint8_t ans;

    USART_ITConfig(USART3, USART_IT_RXNE, DISABLE);
    USART_ITConfig(USART3, USART_IT_TXE, DISABLE);
    ans = (RX[RX_Tail]);
    RX_Tail = (RX_Tail + 1) & (UART_BUFSIZE-1);
    USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
    USART_ITConfig(USART3, USART_IT_TXE, ENABLE);

    return ans;
}

<4>将数据放入发送队列中，这部分我们也要写两个函数，一个是判断发送队列是否有空余地方（head++！=tail，也就是要放的地方和要发送的地方不重合），一个就是把要发送的数放入队列中（也就是把数据放在head处，之后head自加），具体代码如下：

char Usart_FreeSpace(void)
{
    uint16_t tempHead = (TX_Head + 1) & (UART_BUFSIZE-1);
    uint16_t tempTail = TX_Tail;
    return (tempHead != tempTail);
}

char Usart_Putch(uint8_t data)
{
    uint16_t tempTX_Head;
    bool isFree = Usart_FreeSpace();

    if(isFree)
    {
        tempTX_Head = TX_Head;

        USART_ITConfig(USART3, USART_IT_RXNE, DISABLE);
        USART_ITConfig(USART3, USART_IT_TXE, DISABLE);
        TX[tempTX_Head]= data;
        /* Advance buffer head. */
        TX_Head = (tempTX_Head + 1) & (UART_BUFSIZE-1);
        USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
        USART_ITConfig(USART3, USART_IT_TXE, ENABLE);
    }
    return isFree;
}

<5>发送中断，发送中断和接收中断类似，直接上代码了

if(USART_GetITStatus(USART3, USART_IT_TXE) != RESET)
{
    uint16_t tempTX_Tail = TX_Tail;
    if (TX_Head == tempTX_Tail)
    {
        USART_ITConfig(USART3, USART_IT_TXE, DISABLE);
    }
    else
    {
        uint8_t data = TX[TX_Tail];
        USART_SendData(USART3, (unsigned char) data);
        TX_Tail = (TX_Tail + 1) & (UART_BUFSIZE-1);
    }
}

<6>经过上面的步骤之后，我们算是把底层的东西写好了，注意在串口中断处理函数的定义和声明前加入extern "C"的字样（如extern “C” void USART3_IRQHandler(void)），表明我们这是一个C函数，不然你的单片机会一直不能进入中断，归根结底还是因为我们用的C版本的库啊（苦笑），当然，聪明的你会将SysTick_Handler()等你用到的中断函数定义和声明前都加上extern "C"的字样，这样他们也就能工作啦。那么之后我们就要把这些底层函数添加到stm32Hardware的read()函数和write()函数中了，直接代码：

int read(void)
{
    if(Usart_DataAvailable())
    {
        return Usart_Getch();
    }
    else
    {
        return -1;
    }
}
void write(uint8_t* data, int length)
{
    for(int i=0; i

以上就是整个移植的心路历程啦，希望讲的够浅显易懂，最后我也会发出一个demo出来，秉着共同进步的心态，该代码可以在http://download.csdn.net/detail/wubaobao1993/9827708免费的下载到~也希望大神能指出其中的问题，顺便说一下，这个代码是按照我的思路来的，也就是 类似TX_Tail = (TX_Tail + 1) & (UART_BUFSIZE-1)都改为了TX_Tail = (TX_Tail + 1) % (UART_BUFSIZE-0)这样的，实测没有问题，同时提醒广大同僚，如果你的PC端给stm32的信息比较多或者stm32上传的信息比较多，建议把队列的长度加长！

虽然觉得已经写得差不多了，但是还是最后啰嗦一句如何进行测试，聪明的你会先把程序里面的串口部分改为自己板子的串口配置，同时也会把程序中的中断向量表改为自己的串口，之后在ubuntu下打开终端，输入

roscore
rosrun rosserial_python serial_node.py

有可能在过程中会提示你权限的问题，那就用chmod改变一下ttyUSB0的权限（当然并不是所有的电脑都是USB0），如果一切顺利，终端会出现

[INFO] [WallTime: 1493282342.964106] ROS Serial Python Node
[INFO] [WallTime: 1493282342.973830] Connecting to /dev/ttyUSB0 at 57600 baud
[INFO] [WallTime: 1493282345.482891] Note: publish buffer size is 512 bytes
[INFO] [WallTime: 1493282345.483272] Setup publisher on stm_publish [std_msgs/String]
[INFO] [WallTime: 1493282345.486477] Note: subscribe buffer size is 512 bytes
[INFO] [WallTime: 1493282345.486746] Setup subscriber on stm_subscribe [std_msgs/String]

这样的字符，那就是一切顺利啦～之后在新的终端下输入

rostopic pub -r 10 /stm_subscribe std_msgs/String hello

这样会一直发送一个stm_subscribe话题，而stm32接到该话题之后，会发送stm_publish话题回来，之后在新的终端中输入

rostopic echo /stm_publish

这时候就会在终端中打印不停上升的数字，说明我们的移植就成功了！

---

PS

1. 因为网站自动设置积分的机制，现在资源已经涨到了30分。。。想要的同僚们留下邮箱吧，我会在看到的第一时间发送资源过去～
2. 鉴于好多同僚要代码，有时候我的回复也不见得及时，因此把上述代码放入github上，有需要的可以自行下载，如遇到问题，在上面讨论或者在该博客下留言都可以，链接here