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

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

序言：
应学长的要求，我用了一段时间去研究了如何实现32底层与ros上层的通信，在此记录下整体的实现过程以及遇到的问题，废话不多说，开始吧

首先对于几个重要步骤，我想感谢下面几位大哥
stm32与ros的整体框架：https://blog.youkuaiyun.com/wubaobao1993/article/details/70808959
ros自定义msg类型并使用
https://blog.youkuaiyun.com/u013453604/article/details/72903398
实现自定义数据通信：
https://blog.youkuaiyun.com/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_<ArduinoHardware> 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是一致的，顺序也不可以变
以上就是我的整个移植过程，写这篇文档给未来的自己备个份也给后来者一点小小的帮助。
想要程序的可以留下邮箱，我看到之后就会给