ROS学习笔记--移植rosserial到stm32（HAL库、MDK）和ROS通信

前言

ROS通过话题的发布和订阅来进行节点的通信，底层控制器使用rosserial也可以当作一个ROS中的节点，常用的控制器都是基于arduino的，官方也推出了rosserial_stm32。不过官方的是在linux环境下使用sw4stm32，现在使用MDK创建一个工程在博客中记录一下注意事项。

移植

创建CUBEMX工程

使用cubemx创建一个工程，需要使用一个串口和一个定时器。具体创建cubemx工程的步骤这里不再重复，相信大家都会，这里只说明一下注意事项。

串口部分

1. 开启接收和发送的DMA通道，注意接DMA模式为Circular，而发送DMA模式为Normal
2. 打开串口和DMA中断
3. 串口波特率根据需求而定，rosserial_python中默认波特率为57600，可以设置为此波特率。

定时器部分

1. 最好是高级定时器和部分通用定时器，要求ARR寄存器是32位 ，如果没有之后会介绍解决办法
2. 设置时钟分频Prescaler，使定时器计数频率为1K，即CNT加1用时1ms，由于32定时器的PSC寄存器为16位，所以需要调整系统时钟使定时器的时钟为65MHz以下，通常设置64MHz，例如使用TIM2，则调整时钟树使APB1 Timer Clocks为64MHz，然后设置Prescaler为64000-1。若不想降频，后面会介绍方法。
   
3. Counter Period 设为FFFFFFFF，越大越好。
4. 不用开启中断

RosLib的获取

roslib包含了ros相关消息的头文件，获取的方式有很多，可以使用arduino的也可以按rosserial_stm32官方给的方法再linux下直接生成。直接生成的话需要安装sw4stm32，并在其workspace中运行python教本，rosrun rosserial_stm32 make_libraries.py ，过程很简单。

使MDK支持C++

Misc Control 中填写 --cpp11，既可。

阅读部分源码避免进坑

使用rosserial需要注意的地方使时间同步，对32定时器设置有一定的要求，需要看一下roslib内的部分代码来决定定时器的设置。

STM32Hardware.h

 STM32Hardware():
      htim(&htim2), huart(&huart1), rind(0), twind(0), tfind(0){ }

这里指定了需要rosserial用到的定时器和串口，可以根据需求改变

unsigned long time(){ return __HAL_TIM_GET_COUNTER(htim);}

这一函数获取定时器计数，即获取时间

node_handle.h

STM32Hardware.h中的函数都会在node_handle.h文件中被调用，我们主要看time()的调用情况。大概如下所示：

 void syncTime(uint8_t * data)
 {
    std_msgs::Time t;
    uint32_t offset = hardware_.time() - rt_time;

    t.deserialize(data);
    t.data.sec += offset / 1000;
    t.data.nsec += (offset % 1000) * 1000000UL;

    this->setNow(t.data);
    last_sync_receive_time = hardware_.time();
}
void setNow( Time & new_now )
{
	unsigned long ms = hardware_.time();
	sec_offset = new_now.sec - ms/1000 - 1;
	nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
	normalizeSecNSec(sec_offset, nsec_offset);
}

我们可以发现unsigned long ms = hardware_.time();，很明显time()获取的时间单位为毫秒，所以我们需要设计定时器使STM32Hardware.h中time()返回时间单位为毫米。sec为秒，nsec为纳秒。
翻翻对时间的操作会发现没有对定时器溢出产生的错误计时进行修正，所以如果采用ARR寄存器为16位的定时器，其计数周期最大位0xFFFF，即65秒之后定时器溢出，rosserial会产生时间同步错误，而如果使用ARR寄存器为32位的定时器，其最大计数为0xFFFF FFFF,可使用大概一千多个小时，所以不用担心其产生同步错误。

避免进坑

综上所述，roslib中定时器操作有两个坑。

1. time()返回时间单位为毫米
2. 没有对计数溢出进行修正

我们需要进行相应的设计。

坑（一）

最简单的方法就是对muc降频，需要调整系统时钟使定时器的时钟为65MHz以下，通常设置64MHz，设置Prescaler为64000-1。
若不想降频，以stm32f1为例，时钟为72M，可设置定时器的Prescaler为7200-1， Counter Period 设为FFFFFFFF，然后修改STM32Hardware.h中代码，如下

unsigned long time(){ return __HAL_TIM_GET_COUNTER(htim)/10;}

坑（二）

当没有ARR寄存器为32位的定时器的时候，会65秒产生一次时间同步错误，我们可以自行更正计数溢出产生的错误，即开启定时器的更新中断。首先在cubemx中进行操作，之后更改STM32Hardware.h中代码。中断函数部分见下部分

class STM32Hardware
{
	...
public:
	uint32_t timOverflowNum 
	STM32Hardware():
		htim(&htim5), huart(&huart1), rind(0), twind(0), tfind(0),timOverflowNum (0)
	{}
   ...
}; 
void init()
{
	reset_rbuf();
	timOverflowNum = 0；
	HAL_TIM_Base_Start_IT(htim);
}
unsigned long time()
{ 
	return (__HAL_TIM_GET_COUNTER(htim) + timOverflowNum * 0xffff)/10; 
}

中断回调函数的编写

为方便起见，在STM32Hardware.h中STM32Hardware类内添加部分代码

UART_HandleTypeDef* getUart() {return huart;}
TIM_HandleTypeDef* getTim() {return htim;}

回调函数

extern ros::NodeHandle nh;

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if (nh.getHardware()->getTim() == htim) 
	{
		nh.getHardware()->timOverflowNum ++；
	}
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
	if(nh.getHardware()->getUart() == huart)
	{
		nh.getHardware()->flush();
	}
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	if(nh.getHardware()->getUart() == huart)
	{
		 nh.getHardware()->reset_rbuf();
	}
}

使用

// main.c

#include "mainpp.h"
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration----------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART1_UART_Init();
  MX_TIM2_Init();
  /* USER CODE BEGIN 2 */
	setup();
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
	loop();
  /* USER CODE END WHILE */

  /* USER CODE BEGIN 3 */

  }
  /* USER CODE END 3 */

}

// maincpp.cpp
#include 
#include 
#include 

ros::NodeHandle nh;

std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
char hello[] = "Hello world!";

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart){
  nh.getHardware()->flush();
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart){
  nh.getHardware()->reset_rbuf();
}

void setup(void)
{
  nh.initNode();
  nh.advertise(chatter);
}

void loop(void)
{
  HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_3);

  str_msg.data = hello;
  chatter.publish(&str_msg);
  nh.spinOnce();

  HAL_Delay(1000);
}

    
//mainpp.h
#ifndef MAINPP_H_
#define MAINPP_H_

#ifdef __cplusplus
 extern "C" {
#endif

void setup(void);
void loop(void);

#ifdef __cplusplus
}
#endif

#endif /* MAINPP_H_ */

下载

roslib下载链接