**实现STM32与ROS上层自定义数据通信**

实现STM32与ROS上层自定义数据通信

序言：
应学长的要求，我用了几天的时间去研究了如何实现32底层与ros上层的通信（本人对ros也是一知半解。。。），在此记录下整体的实现过程以及遇到的问题，废话不多说，开始吧

首先对于几个重要步骤，我想感谢下面几位大哥
stm32与ros的整体框架：https://blog.csdn.net/wubaobao1993/article/details/70808959
ros自定义msg类型并使用
https://blog.csdn.net/u013453604/article/details/72903398
实现自定义数据通信：
https://blog.csdn.net/wanzew/article/details/80030768

虽然ros提供的demo是针对arduino的，但是通过一定的移植就可以实现32与ros的通信

下面是具体的步骤

首先安装所需要的软件（我的是kinetic版本）
sudo apt-get install ros-kinetic-rosserial-arduino
sudo apt-get install ros-kinetic-rosserial
接下来是源码安装：
cd /src
git clone https://github.com/ros-drivers/rosserial.git
cd
catkin_make
catkin_make install
最后是在sketchbook中生成ros_lib
cd /libraries
rm -rf ros_lib
rosrun rosserial_arduino make_libraries.py .（不要忘了最后这个点）
注意：如果是想生成自定义数据的话，一定要在生成ros_lib之前把自己的msg文件写好

当生成ros_lib之后，把ros_lib直接复制到自己的32的程序中，这个时候整个程序都是不能用的，我们首先先修改ros.h里的东西
#ifndef ROS_H
#define ROS_H

#include “ros/node_handle.h”

#if defined(ESP8266) or defined(ESP32) or defined(ROSSERIAL_ARDUINO_TCP)
#include “ArduinoTcpHardware.h”
#else
#include “ArduinoHardware.h”
#endif

namespace ros
{
#if defined(AVR_ATmega8) or defined(AVR_ATmega168)
/* downsize our buffers */
typedef NodeHandle_<ArduinoHardware, 6, 6, 150, 150> NodeHandle;

#elif defined(AVR_ATmega328P)

typedef NodeHandle_<ArduinoHardware, 25, 25, 280, 280> NodeHandle;

#elif defined(SPARK)

typedef NodeHandle_<ArduinoHardware, 10, 10, 2048, 2048> NodeHandle;

#else

typedef NodeHandle_ NodeHandle; // default 25, 25, 512, 512

#endif
}

#endif

这是刚移植过来之后的原始样子，这时候就需要我们去修改，而不是直接使用，里面的数据的定义也是可以修改的，我在这里的程序是这样的

#ifndef ROS_H
#define ROS_H

#include “ros/node_handle.h”
#include “STM32Hardware.h”

namespace ros
{
typedef NodeHandle_<STM32Hardware, 25, 25, 1024, 1024> NodeHandle;
}

#endif
这使我们的NodeHandle有25个Publishers，25个Subscriber，1024个字节用于输入缓冲区，1024个字节用于输出缓冲区。

接下来我们就需要实现32的串口部分，在这里我是用的寄存器配置的，波特率设置的57600，因为ros默认的就是57600，这部分我就不贴代码了，32的串口程序应该很好找，有一点要注意的是，我在根据一个网站写的时候用的串口的发送完成中断，出现了Unable to sync with device; possible link problem or link software version mismatch such as hyd的问题，原因就是因为我在中断中也写了发送函数导致数据错位了。在写好串口之后，我们就需要去在STM32Hardware.h里实现一些接口函数，这个文件ros原生不会提供，需要我们自己实现，下面就是我的实现过程

#ifndef STM32HARDWARE_H
#define STM32HARDWARE_H

#include <stdint.h>
#include “usart.h”
#include “delay.h”
class STM32Hardware
{
public:
STM32Hardware(){};
void init(void)
{
baud_ = 57600;
uart_init(baud_);
}

    int getBaud(){return baud_;}
    
    void setBaud(long baud)
    {
        this->baud_= baud;
    }

int read(void)
{
if(Usart_DataAvailable())
{
return Usart_Getch();
}
else
{
return -1;
}
}

void write(uint8_t* data, int length)
{
for(int i=0; i<length; i++)
{
while(!Usart_FreeSpace()){}
Usart_Putch(data[i]);
}
}
unsigned long time(void){return System_GetTime();}

protected:        
    long baud_;

};

#endif
注意：函数名字一定不要修改，这都是对应ros提供的函数接口名字，在实现这些之后，我们就可以去写我们的主函数

#define USE_USBCON
#include “sys.h”
#include “usart.h”
#include “delay.h”
#include “stm32f10x.h”
#include “config.h”
#include “stdio.h”
#include “std_msgs/String.h”
#include “ros.h”
#include “Stm32Hardware.h”
#include “timer.h”
#include <ziki_msgs/gobot_enco.h>
#include <ziki_msgs/imu.h>
void msgCallBack(const std_msgs::String& msg);
std_msgs::String msg1;
std_msgs::String msg2;
riki_msgs::gobot_enco msg3;
riki_msgs::Imu msg4;

ros::NodeHandle nh;
ros::Subscriber<std_msgs::String> sub(“stm_subscribe”, msgCallBack);
ros::Publisher pub(“gobot_enco”,&msg3);
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)
{
Stm32_Clock_Init(9);
System_Init();
//delay_init(72);
uart_init(57600);

nh.initNode();
nh.advertise(pub);

nh.subscribe(sub);
char hello[13] = “hello world!”;
while(1)
{
msg3.l_enc=122;
msg3.r_enc=100;
//USART1->DR=‘a’;
msg2.data = hello;
//pub.publish(&msg3);
nh.spinOnce();
delay_ms(100);
}
}

这里我用了回调函数发送数据，但是在这里我碰到了一个bug，在ros上层收到我发送的主题之后却接不到数据，因此我改成了在while里发送，当这些都完成之后，我们就可以去ros端接收数据了。
执行下面两个命令：

roscore
rosrun rosserial_python serial_node.py
就可以接收数据啦
之后我们可以通过rosmsg show 你的主题名字就可以看到你发布的数据了

完成这些之后，我们接着来实现自定义数据的发布：

还记得我之前说的在生成ros_lib前定义好你想自定义的数据，这样在生成ros_lib之后里面就有我们需要的自定义数据类型的.h文件了，这时我们把它放在我们的32环境中，调用它，记住.h文件中生成的是一个类，所以我们要按照定义类类型的数据去定义，然后赋值最后通过pub函数发布出去，这些在我的主函数中都有体现。

在自定义数据发布到ros的时候一定要注意，要保证ros那边也有你的这种数据类型，且你的主题一定是你定义的msg的名字，否则就会出现问题。

（2020.1.10）补充：msg文件一定要和生成的.h是一致的，顺序也不可以变
以上就是我的整个移植过程，写这篇文档给未来的自己备个份也给后来者一点小小的帮助。
想要程序的可以留下邮箱，我看到之后就会给