# <center>ROS和STM32通訊 Ⅰ</center>
###### <center>rosserial實作</center>
<!--### **<u>前情提要</u>**
我來自清大機器人團隊DIT Robotics，目前大二。在Eurobot2022歐洲自動化機器人比賽中，其中負責的項目之一是處理ROS和STM32的通訊。之前從未接觸過相關技術，在ROS和STM32領域也是初學者。一開始上網查資料，發現網路上很少詳細的步驟教學，資料也都零零散散，自己摸索了很久，也尋求很多人的幫助，決定寫下完整實作紀錄。</br>-->
---
### **<u>rosserial簡介</u>**
rosserial是透過在ROS端運行一節點，將訊息序列化，使得STM32可以作為一節點，透過ROS master可直接與ROS端其他節點進行通信。</br>
**我使用的是Ubuntu 20.04 ROS Noetic 搭配 STM32F429ZI 開發板。**
---
### **<u>ROS端安裝設定</u>**</br>
1. 打開終端機安裝兩個package(注意版本號)
```cmake=
sudo apt-get install ros-noetic-rosserial ros-noetic-rosserial-arduino
```
2. 選定一個位置下載ros-drivers(注意版本號)
```cmake=
git clone https://github.com/ros-drivers/rosserial.git --branch noetic-devel
```
3. 上一個步驟會下載名稱為rosserial的資料夾，移至該資料夾內。
```cmake=
cd ~/your_directory/rosserial
```
4. 利用指令ls，列出資料夾內內容。其中一個package為rosserial_python，將其複製到平常使用的工作區裡的src資料夾。
```cmake=
sudo cp -avr rosserial_python ~/your_ws/src
```
5. 完成ROS端設定！
---
### **<u>STM端安裝設定</u>(這裡使用STM32CubeIDE)**</br>
1. 在左列project selector中，選擇要使用rosserial的專案，按下右鍵選擇"Convert to C++"
2. 生成rosserial需要用到的Library
(1) 首先，移動到Ubuntu，生成Library，步驟如下。
(2) 移至平常使用的工作區裡的src資料夾。
```cmake=
cd ~/your_ws/src
```
(3) 下載rosserial_stm32 package</br>
```cmake=
git clone https://github.com/yoneken/rosserial_stm32.git
```
(4) 移至根目錄，進行編譯，並source一次devel資料夾裡的setup.bash。</br>
```cmake=
cd ..
catkin_make
source devel/setup.bash
```
(5) 移至剛下載的package內的src內的rosserial_stm32。</br>
```cmake=
cd ~/your_ws/src/rosserial_stm32/src/rosserial_stm32
```
(6) make_libraries文件內容需修改，**<font color=red>print需加上前後括號</font>**。</br>
```cmake=
sudo nano make_libraries.py
```
<div style="text-align: center">
<img src="https://i.imgur.com/O0XeiOe.png"/>
</div>
(7)在package內的src建立一資料夾，命名為Inc(不要自行更改資料夾名稱)。這裡我選擇在rosserial_stm32這個package。
```cmake=
cd ~/your_ws/src/rosserial_stm32
mkdir Inc
```
(8) 生成Library，後面接上Library生成位置。這裡生成在上步驟建立的Inc資料夾內。</br>
```cmake=
rosrun rosserial_stm32 make_libraries.py ~/your_ws/src/rosserial_stm32
```
(9) 會出現兩個選擇，選擇執行在src資料夾內的 make_libraries.py，通常為2。</br>
(10) 移至package內的src內的Inc，使用ls指令列出資料夾內容，可以看到生成的Library會放在這裡。</br>
```cmake=
cd ~/your_ws/src/rosserial_stm32/Inc
```
3. 將上步驟生成的Library，移至/你的STM專案資料夾/Core/Inc內(直接複製進資料夾)，並回到STM32CubeIDE中，在左列project selector中，選擇該專案，按下右鍵選擇"Refresh"，按下下拉選單可以看見Library已被成功加入。
<div style="text-align: center">
<img src="https://i.imgur.com/IFtVdRa.png"/>
</div>
4. 設置ioc檔
(1) 配置UART腳位。這裡選擇USART3，TX腳位為PB10，RX腳位為PD9。
(2) 選取左列Connectivity USART3。
 ● Mode：Asynchronous。
 ● NVIC：勾選 USART3 global interrupt。
 ● DMA：Add USART3_TX 和 USART3_RX，將 priority 設為 High。
 ● parameter settings：115200 Bits/s (自行設定鮑率)。
(3) 選取左列System Core裡的RCC，將HSE選為BYPASS Clock Source。
(4) 設定Clock Configuration。
<div style="text-align: center">
<img src="https://i.imgur.com/3OIYpzd.png"/>
</div>
5. 在Core/src新增一個檔案mainpp.cpp，並在main.c 中 include mainpp.h，將函式setup和loop寫進main.c。
**main.c**
```cpp=
int main(void){
...
MX_DMA_Init();
MX_USART3_UART_Init();
/* USER CODE BEGIN 2 */
setup();
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
loop();
}
/* USER CODE END 3 */
...
```
**mainpp.cpp**（這裡的寫法和在ROS發佈、訂閱topic，基本上概念差不多。）
```cpp=
#include "mainpp.h"
#include "ros.h"
#include "geometry_msgs/Twist.h"
void vel_callback(const geometry_msgs::Twist &msg)
{
vel[0] = msg.linear.x; //double vel[3]宣告在mainpp.h
vel[1] = msg.linear.y;
vel[2] = msg.angular.z;
}
ros::NodeHandle nh;
ros::Subscriber<geometry_msgs::Twist> sub("give_car_speed", vel_callback);
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.subscribe(sub);
}
void loop(void)
{
nh.spinOnce();
}
```
**mainpp.h**
```cpp=
/*
* mainpp.h
*
* Created on: 2018/01/17
* Author: yoneken
*/
#ifndef MAINPP_H_
#define MAINPP_H_
#ifdef __cplusplus
extern "C"
{
#endif
void setup(void);
void loop(void);
double vel[3];
#ifdef __cplusplus
}
#endif
#endif /* MAINPP_H_ */
```
6. 在Core/Inc裡修改STM32HardWare.h。
(1) 選擇使用的板子型號。
(2) 選擇使用的UART。
<div style="text-align: center">
<img src="https://i.imgur.com/ak9uK5k.png"/>
</div>
</br>
:::info
**小補充**
  如果你的板子型號不在他原本預設的裡面，可以自己改寫成以下幾行。**假設我使用的開發板型號是STM32H723ZG。**
```cpp=
#define STM32H7xx //上圖 Line 38
...
#ifdef STM32H7xx
#include "stm32H7xx_hal.h"
#include "stm32H7xx_hal_uart.h"
#endif /* STM32H7xx */
```
:::
7. 完成STM端設定！</br>
---
### **<u>開始測試rosserial</u>**</br>
1. **硬體配置**
<div style="text-align: center">
<img src="https://i.imgur.com/kDysIuI.png"/>
</div>
1. **開始測試**
```cmake=
rosrun rosserial_python serial_node.py _baud:=115200
```
如果成功，會顯示以下畫面。列出Baud Rate，發佈和訂閱的topic name，以及buffer size。
<div style="text-align: center">
<img src="https://i.imgur.com/OQJA2vu.png"/>
</div></br>
恭喜，可以開始將rosserial應用在自己的專案了！
:::info
一般會將STM端的publish寫進中斷，比較好控制發佈頻率，之後會再寫一篇介紹如何使用中斷功能搭配UART通訊！</br>
:::
---
### **<u>DEBUG</u>**</br>
1. **CP2102 USB to TTL port問題**
USB不一定會連接在預設的/dev/ttyUSB0下，可以利用以下指令檢查。</br>
```cmake=
dmesg | grep tty
```
假設USB連接在/dev/ttyUSB1下，執行指令時可以加上port。</br>
```cmake=
rosrun rosserial_python serial_node.py _baud:=115200 _port:=/dev/ttyUSB1
```
權限不足時，可以輸入以下指令開啟port權限。</br>
```cmake=
sudo chmod 777 /dev/ttyUSB0
```
1. **ioc檔問題**
我遇到一個比較奇怪的問題是，ioc檔儲存後自動生成的程式碼有錯誤。結論是DMA的init需在UART的init之前，我的自動生成會顛倒。</br>
```cmake=
MX_DMA_Init();
MX_USART3_UART_Init();
```
1. **硬體問題1**
開發板上的TX接到CP2102的RX，開發板上的RX接到CP2102的TX。
1. **硬體問題2**
我是使用VMWare虛擬機開啟Ubuntu。若錯過USB選擇連接的視窗，需自行將CP2102 USB to TTL連上虛擬機。
選擇右上角Player -> Removable Devices -> Silicon CP2102 USB to UART Bridge Controller -> Connect。
1. **Error code-1**
Error code：Tried to publish before configured, topic id 125
<div style="text-align: center">
<img src="https://i.imgur.com/cibyAvC.png"/>
</div>
解決方法：RESET STM32。
1. **Error code-2**
Error code：message larger than buffer</br>
解決方法：調整buffer size。</br>
(1)修改STMHardware.h (位置：/Core/Inc/STMHardware.h)
```cpp=
...
class STM32Hardware {
protected:
UART_HandleTypeDef *huart;
const static uint16_t rbuflen = 1024; // 512 to 1024
uint8_t rbuf[rbuflen];
uint32_t rind;
inline uint32_t getRdmaInd(void){ return (rbuflen - __HAL_DMA_GET_COUNTER(huart->hdmarx)) & (rbuflen - 1); }
const static uint16_t tbuflen = 1024; // 512 to 1024
uint8_t tbuf[tbuflen];
uint32_t twind, tfind;
...
```
(2)修改node_handle.h (位置：/Core/Inc/ros/node_handle.h)
```cpp=
...
template<class Hardware,
int MAX_SUBSCRIBERS = 25,
int MAX_PUBLISHERS = 25,
int INPUT_SIZE = 1024, // 512 to 1024
int OUTPUT_SIZE = 1024> // 512 to 1024
...
```
---
下一篇：[ROS和STM32通訊 Ⅱ - rosserial搭配STM中斷功能](https://hackmd.io/@JINGCCC/rosserial_2)
---
<h5 style="text-align:right">寫於2022/1/28</h5>
Sign in via Google
Sign in via Facebook
Sign in via X(Twitter)
Sign in via GitHub
Sign in via Dropbox
Sign in with Wallet
Connect another wallet