RT-Thread+STM32實現智能車目標識別系統的教程

引言

這篇文檔主要介紹 RT-Thread 如何使用串口或者無線和 ROS 連接，會包含這麼些內容：

第一部分：ROS 環境搭建

第二部分：RT-Thread rosserial 軟體包

第二部分：RT-Thread 添加 USART2 和 PWM

第三部分：RT-Thread 使用 ESP8266 AT 固件聯網

這裡先介紹一下什麼是 ROS？為什麼要和 ROS 連接？

機器人作業系統 ROS (Robots Operating System) 最早是史丹福大學的一個軟體框架，現在不管是工業機器人，還是娛樂用的機器人都運行著 ROS。

圖片來源網絡，如有侵權請聯繫刪除

一個機器人通常有很多個部件、傳感器，為了保證機器人不會因為某一個傳感器故障，導致整個系統癱瘓，所以採用了分布式的節點，利用不同節點之間的通訊收集傳感器數據和控制指令，這篇文檔後面會使用到的通訊協議就是 rosserial。和 ROS 連接的好處在於，一方面由 ROS 管理各個機器人節點更穩定，另一方面 ROS 現在已經有了非常多成熟的軟體包，使用 ROS 就可以非常方便的為自己的機器人添加攝像頭圖像識別、雷射雷達建圖導航等高級功能。不過這篇文檔只會涉及 RT-Thread 和 ROS 建立基本的連接，實現小車的運動控制，之後可能會有後續文檔介紹如何連接雷射雷達建圖，並進行全局路徑規劃。

這篇文章假定大家都已經會用 RT-Thread 的 env 工具下載軟體包，生成項目上傳固件到 stm32 上，並且熟悉 Ubuntu 的基本使用。

1 ROS 簡介

這裡的開發環境搭建其實是需要搭建 2 份，一份是小車上的 ARM 開發板 (樹莓派，NanoPi 什麼的)，另一個則是自己的電腦，因為我們希望把電腦作為 ROS 從節點，連接到小車上的 ROS 主節點，不過開發板和電腦的 ROS 安裝是一模一樣的。

既然要和 ROS 連接，那麼首先就得要有一個正常運行的 ROS。安裝 ROS 其實非常簡單，這裡推薦使用 Ubuntu 18 (開發板推薦系統用 Armbian)，因為官方對 Ubuntu 的支持優先級是最高的，安裝教程也可以參照 官網：http://wiki.ros.org/melodic/Installation/Ubuntu只需要輸入下面的 4 行命令，就在 Ubuntu 上裝好了 ROS。

1sudo sh -c 'echo "deb https://mirror.tuna.tsinghua.edu.cn/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
2sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
3sudo apt update
4sudo apt install ros-melodic-ros-base

上面我使用了清華大學的鏡像源，這樣從國內下載 ROS 會快很多，而且我只安裝了 ROS 的基本軟體包，沒有安裝圖形化軟體包 gviz，gazebo 什麼的，因為後面也沒有用到。1.2 ROS 環境初始化ROS 安裝好之後還需要進行初始化，不過也是只有短短幾行命令：

1sudo rosdep init
2rosdep update
3
4echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
5source ~/.bashrc

啟動 ROS 的話我們需要確保它是常駐後臺運行的，所以我們可以使用 tmux：

在 tmux 裡啟動了 ROS 主節點後，我們就可以 Ctrl + B D 退出了，而 ROS 主節點依舊在後臺運行。

1.4 參考文獻Armbian:https://www.armbian.com/ROS Melodic 安裝:http://wiki.ros.org/melodic/Installation/Ubuntu
2 RT-Thread 串口連接 ROS這一部分會介紹如何使用串口將運行著 RT-Thread 的 STM32 開發板和運行著 ROS 的 ARM 開發板連接，看起來差不多就是這樣。

這裡說明一下不同開發板的分工，STM32 運行著 RT-Thread 負責控制電機，接收傳感器信息；ARM 運行著 ROS 負責進行全局控制，例如給小車發出前進的指令。
2.1 RT-Thread 配置首先我們需要打開 usart2，因為 usart1 被 msh 使用了，保留作為調試還是挺方便的。在 CubeMX 裡我打開了 USART2，另外還打開了 4 路 PWM，因為我後面使用了 2 個電機，每個電機需要 2 路 PWM 分別控制前進和後退。接下來還需要在 menuconfig 裡面打開對應的選項，考慮到有的開發板默認的 bsp 可能沒有這些選項，可以修改 board/Kconfig 添加下面的內容。

1menuconfig BSP_USING_UART
2    bool "Enable UART"
3    default y
4    select RT_USING_SERIAL
5    if BSP_USING_UART
6        config BSP_USING_UART1
7            bool "Enable UART1"
8            default y
9
10        config BSP_UART1_RX_USING_DMA
11            bool "Enable UART1 RX DMA"
12            depends on BSP_USING_UART1 && RT_SERIAL_USING_DMA
13            default n
14
15        config BSP_USING_UART2
16            bool "Enable UART2"
17            default y
18
19        config BSP_UART2_RX_USING_DMA
20            bool "Enable UART2 RX DMA"
21            depends on BSP_USING_UART2 && RT_SERIAL_USING_DMA
22            default n
23    endif

1menuconfig BSP_USING_PWM
2    bool "Enable pwm"
3    default n
4    select RT_USING_PWM
5    if BSP_USING_PWM
6    menuconfig BSP_USING_PWM3
7        bool "Enable timer3 output pwm"
8        default n
9        if BSP_USING_PWM3
10            config BSP_USING_PWM3_CH1
11                bool "Enable PWM3 channel1"
12                default n
13            config BSP_USING_PWM3_CH2
14                bool "Enable PWM3 channel2"
15                default n
16            config BSP_USING_PWM3_CH3
17                bool "Enable PWM3 channel3"
18                default n
19            config BSP_USING_PWM3_CH4
20                bool "Enable PWM3 channel4"
21                default n
22        endif
23    endif

這樣我們在 env 下就可以看到有對應的配置了,

除此之外，我們還需要選擇 rosserial 軟體包：

可以看到上面默認的串口就是 USART2，這樣我們就可以生成對應的工程了：

1pkgs --update
2scons --target=mdk5 -s

如果我們打開 Keil 項目，首先需要把 main.c 修改為 main.cpp，因為 rosserial 很多數據格式的定義都是用 C++ 寫的，所以如果要使用 rosserial 庫，我們先得把後綴改為 cpp，這樣 Keil 就會用 C++ 編譯器編譯。

下面是 main.cpp 的內容，其實就是初始化了電機，然後發布了 2 個話題 (topic)，一個是 /vel_x 告訴 ROS 當前小車的速度，一個是 /turn_bias 告訴 ROS 當前小車的旋轉速度。同時又訂閱了一個話題 /cmd_vel，用來接收從 ROS 發出的控制指令。代碼不是特別長，我也添加了一些注釋，所以這裡就不一行行分析了。

1#include <rtthread.h>
2#include <rtdevice.h>
3#include <board.h>
4
5#include <ros.h>
6#include <std_msgs/Float64.h>
7#include <geometry_msgs/Twist.h>
8#include "motors.h"
9
10ros::NodeHandle  nh;
11MotorControl mtr(1, 2, 3, 4);   //Motor
12
13bool msgRecieved = false;
14float velX = 0, turnBias = 0;
15char stat_log[200];
16
17// 接收到命令時的回調函數
18void velCB( const geometry_msgs::Twist& twist_msg) 
19{
20  velX = twist_msg.linear.x;
21  turnBias = twist_msg.angular.z;
22  msgRecieved = true;
23}
24
25//Subscriber
26ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", velCB );
27
28//Publisher
29std_msgs::Float64 velX_tmp;
30std_msgs::Float64 turnBias_tmp;
31ros::Publisher xv("vel_x", &velX_tmp);
32ros::Publisher xt("turn_bias", &turnBias_tmp);
33
34static void rosserial_thread_entry(void *parameter)
35{
36    //Init motors, specif>y the respective motor pins
37    mtr.initMotors();
38
39    //Init node>
40    nh.initNode();
41
42    // 訂閱了一個話題 /cmd_vel 接收控制指令
43    nh.subscribe(sub);
44
45    // 發布了一個話題 /vel_x 告訴 ROS 小車速度
46    nh.advertise(xv);
47
48    // 發布了一個話題 /turn_bias 告訴 ROS 小車的旋轉角速度
49    nh.advertise(xt);
50
51    mtr.stopMotors();
52
53    while (1)
54    {
55      // 如果接收到了控制指令
56      if (msgRecieved) 
57      {
58        velX *= mtr.maxSpd;
59        mtr.moveBot(velX, turnBias);
60        msgRecieved = false;
61      }
62
63      velX_tmp.data = velX;
64      turnBias_tmp.data = turnBias/mtr.turnFactor;
65
66      // 更新話題內容
67      xv.publish( &velX_tmp );
68      xt.publish( &turnBias_tmp );
69
70      nh.spinOnce();
71    }
72}
73
74int main(void)
75{
76    // 啟動一個線程用來和 ROS 通信
77    rt_thread_t thread = rt_thread_create("rosserial",     rosserial_thread_entry, RT_NULL, 2048, 8, 10);
78    if(thread != RT_NULL)
79    {
80        rt_thread_startup(thread);
81        rt_kprintf("[rosserial] New thread rosserial\n");
82    }
83    else
84    {
85        rt_kprintf("[rosserial] Failed to create thread rosserial\n");
86    }
87    return RT_EOK;
88}

另外還有對應的電機控制的代碼，不過這個大家的小車不同，驅動應當也不一樣，我這裡由於小車電機上沒有編碼器，所以全部是開環控制的。

1#include <rtthread.h>
2
3class MotorControl {
4  public:
5    //Var
6    rt_uint32_t  maxSpd;
7    float moveFactor;
8    float turnFactor;
9
10    MotorControl(int fl_for, int fl_back,
11                 int fr_for, int fr_back);
12    void initMotors();
13    void rotateBot(int dir, float spd);
14    void moveBot(float spd, float bias);
15    void stopMotors();
16  private:
17    struct rt_device_pwm *pwm_dev;
18    //The pins
19    int fl_for;
20    int fl_back;
21    int fr_for;
22    int fr_back;
23    int bl_for;
24    int bl_back;
25    int br_for;
26    int br_back;
27};

1#include <rtthread.h>
2#include <rtdevice.h>
3#include "motors.h"
4
5#define PWM_DEV_NAME "pwm3"
6
7MotorControl::MotorControl(int fl_for, int fl_back,
8                           int fr_for, int fr_back) 
9{
10    this->maxSpd = 500000;
11    this->moveFactor = 1.0;
12    this->turnFactor = 3.0;
13
14    this->fl_for = fl_for;
15    this->fl_back = fl_back;
16
17    this->fr_for = fr_for;
18    this->fr_back = fr_back;
19}
20
21void MotorControl::initMotors() {
22    /* 查找設備 */
23    this->pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
24    if (pwm_dev == RT_NULL)
25    {
26        rt_kprintf("pwm sample run failed! can't find %s device!\n", PWM_DEV_NAME);
27    }
28    rt_kprintf("pwm found %s device!\n", PWM_DEV_NAME);
29    rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
30    rt_pwm_enable(pwm_dev, fl_for);
31
32    rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
33    rt_pwm_enable(pwm_dev, fl_back);
34
35    rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
36    rt_pwm_enable(pwm_dev, fr_for);
37
38    rt_pwm_set(pwm_dev, fr_back, maxSpd, 0);
39    rt_pwm_enable(pwm_dev, fr_back);
40}
41
42// 小車運動
43void MotorControl::moveBot(float spd, float bias) {
44    float sL = spd * maxSpd;
45    float sR = spd * maxSpd;
46    int dir = (spd > 0) ? 1 : 0;
47
48    if(bias != 0)
49    {
50        rotateBot((bias > 0) ? 1 : 0, bias);
51        return;
52    }
53
54    if( sL < -moveFactor * maxSpd)
55    {
56        sL = -moveFactor * maxSpd;
57    }
58    if( sL > moveFactor * maxSpd)
59    {
60        sL = moveFactor * maxSpd;
61    }
62
63    if( sR < -moveFactor * maxSpd)
64    {
65        sR = -moveFactor * maxSpd;
66    }
67    if( sR > moveFactor * maxSpd)
68    {
69        sR = moveFactor * maxSpd;
70    }
71
72    if (sL < 0) 
73    {
74        sL *= -1;
75    }
76
77    if (sR < 0) 
78    {
79        sR *= -1;
80    }
81
82    rt_kprintf("Speed Left: %ld\n", (rt_int32_t)sL);
83    rt_kprintf("Speed Right: %ld\n", (rt_int32_t)sR);
84
85    if(dir)
86    {
87        rt_pwm_set(pwm_dev, fl_for, maxSpd, (rt_int32_t)sL);
88        rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
89        rt_pwm_set(pwm_dev, fr_for, maxSpd, (rt_int32_t)sR);
90        rt_pwm_set(pwm_dev, fr_back, maxSpd, 0);
91    }
92    else
93    {
94        rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
95        rt_pwm_set(pwm_dev, fl_back, maxSpd, (rt_int32_t)sL);
96        rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
97        rt_pwm_set(pwm_dev, fr_back, maxSpd, (rt_int32_t)sR);
98    }
99
100    rt_thread_mdelay(1);
101}
102
103
104// 小車旋轉
105void MotorControl::rotateBot(int dir, float spd) {
106    float s = spd * maxSpd;
107    if (dir < 0) 
108    {
109        s *= -1;
110    }
111    if(dir)
112    {
113        // Clockwise
114        rt_pwm_set(pwm_dev, fl_for, maxSpd, (rt_int32_t)s);
115        rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
116        rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
117        rt_pwm_set(pwm_dev, fr_back, maxSpd, (rt_int32_t)s);
118    }
119    else
120    {
121        // Counter Clockwise
122        rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
123        rt_pwm_set(pwm_dev, fl_back, maxSpd, (rt_int32_t)s);
124        rt_pwm_set(pwm_dev, fr_for, maxSpd, (rt_int32_t)s);
125        rt_pwm_set(pwm_dev, fr_back, maxSpd, 0);
126    }
127    rt_thread_mdelay(1);
128}
129
130//Turn off both motors
131void MotorControl::stopMotors() 
132{
133    rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
134    rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
135    rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
136    rt_pwm_set(pwm_dev, fr_back, maxSpd, 0);
137}

一共只需要這麼一點代碼就可以實現和 ROS 的連接了，所以其實 ROS 也不是那麼神秘，它就是因為簡單好用所以才這麼受歡迎的。既然 RT-Thread 已經配置好了，下一步就是 ROS 的配置了。2.2 ROS 配置我們把上面 RT-Thread 的固件傳到板子上以後，可以用一個 USB-TTL 一邊和 STM32 控制板的 USART2 連接，另一邊插到 ARM 控制板的 USB 口，接下來就可以建立連接了，在 ARM 板上輸入命令：

1$ rosrun rosserial_python serial_node.py /dev/ttyUSB0

1tpl@nanopineoplus2:~$ rosrun rosserial_python serial_node.py /dev/ttyUSB0
2[INFO] [1567239474.258919]: ROS Serial Python Node
3[INFO] [1567239474.288435]: Connecting to /dev/ttyUSB0 at 57600 baud
4[INFO] [1567239476.425646]: Requesting topics...
5[INFO] [1567239476.464336]: Note: publish buffer size is 512 bytes
6[INFO] [1567239476.471349]: Setup publisher on vel_x [std_msgs/Float64]
7[INFO] [1567239476.489881]: Setup publisher on turn_bias [std_msgs/Float64]
8[INFO] [1567239476.777573]: Note: subscribe buffer size is 512 bytes
9[INFO] [1567239476.785032]: Setup subscriber on cmd_vel [geometry_msgs/Twist]

2.3 ROS 控制小車既然已經成功建立連接了，下一步就是寫小車控制的代碼了。

1$ mkdir catkin_workspace && cd catkin_workspace
2$ catkin_init_workspace

1$ cd src
2$ catkin_create_pkg my_first_pkg rospy

這樣就會自動在 src 目錄創建一個 ROS 軟體包了。我們在 catkin_workspace/src/my_first_pkg/src 目錄下新建一個文件 ros_cmd_vel_pub.py：

1#!/usr/bin/python
2
3import rospy
4from geometry_msgs.msg import Twist
5from pynput.keyboard import Key, Listener
6
7vel = Twist()
8vel.linear.x = 0
9
10def on_press(key):
11
12    try:
13        if(key.char == 'w'):
14            print("Forward")
15            vel.linear.x = 0.8
16            vel.angular.z = 0
17
18        if(key.char == 's'):
19            print("Backward")
20            vel.linear.x = -0.8
21            vel.angular.z = 0
22
23        if(key.char == 'a'):
24            print("Counter Clockwise")
25            vel.linear.x = 0
26            vel.angular.z = -0.8
27
28        if(key.char == 'd'):
29            print("Clockwise")
30            vel.linear.x = 0
31            vel.angular.z = 0.8
32
33        return False
34
35    except AttributeError:
36        print('special key {0} pressed'.format(key))
37        return False
38
39def on_release(key):
40    vel.linear.x = 0
41    vel.angular.z = 0
42
43    return False
44
45# Init Node
46rospy.init_node('my_cmd_vel_publisher')
47pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
48
49# Set rate
50rate = rospy.Rate(10)
51
52listener = Listener(on_release=on_release, on_press = on_press)
53
54while not rospy.is_shutdown():
55    print(vel.linear.x)
56    pub.publish(vel)
57    vel.linear.x = 0
58    vel.angular.z = 0
59    rate.sleep()
60
61    if not listener.running:
62        listener = Listener(on_release=on_release, on_press = on_press)
63        listener.start()

這就是我們的 python 控制程序了，可以使用鍵盤的 wasd 控制小車前進後退，順時針、逆時針旋轉。我們需要給它添加可執行權限：

1$ chmod u+x ./ros_cmd_vel_pub.py

這樣就可以編譯軟體包了，在 catkin_worspace 目錄下。

1$ catkin_make
2$ source devel/setup.bash

1rosrun my_first_pkg ros_cmd_vel_pub.py

可以看到用 ROS 實現小車控制其實代碼量並不算多，只需要在自己小車原有的代碼上發布一些話題，告訴 ROS 小車當前的狀態，並且訂閱一個話題接收 ROS 的控制指令就可以了。2.4 參考文獻ros-pibot:https://github.com/wuhanstudio/ros-pibot
3 RT-Thread 無線連接 ROS3.1 rosserial 配置其實無線連接和有線連接幾乎是一模一樣的，只不過是先用 ESP8266 使自己的控制板能連上網，然後用 tcp 連接和 ROS 通信，關於 RT-Thread 使用 ESP8266 上網的教程可以參照 官網:https://www.rt-thread.org/document/site/application-note/components/at/an0014-at-client/，非常詳細了，我這裡就不重複了。確保開發板有網絡連接後，我們就可以在 rosserial 裡面配置為使用 tcp 連接：

我們只需要在上一部分的 main.cpp 裡添加一行代碼：

1// 設置 ROS 的 IP 埠號
2nh.getHardware()->setConnection("192.168.1.210", 11411);
3
4// 添加在節點初始化之前
5nh.initNode();

開發板就能通過 tcp 連接和 ROS 通信了，非常方便。3.2 ROS 配置由於我們使用了 tcp 連接，所以 ROS 上自然也要開啟一個伺服器了，之前是使用的串口建立連接，現在就是使用 tcp 了：

1$ rosrun rosserial_python serial_node.py tcp

其他的代碼完全不需要改變，這樣我們就實現了一個 ROS 無線控制的小車了。

3.3 參考文獻
4 總結這裡再總結一下，其實 RT-Thread 使用 rosserial 軟體包和 ROS 建立連接非常簡單，只需要在自己小車原有代碼的基礎上發布一些消息，告訴 ROS 小車當前的狀態，以及訂閱來自 ROS 的控制指令就可以了。