|
引言 這篇文檔主要介紹 RT-Thread 如何使用串口或者無線和 ROS 連接，會包含這么些內容： 第一部分：ROS 環境搭建 第二部分：RT-Thread rosserial 軟件包 第二部分：RT-Thread 添加 USART2 和 PWM 第三部分：RT-Thread 使用 ESP8266 AT 固件聯網 這里先介紹一下什么是 ROS？為什么要和 ROS 連接？ 機器人操作系統 ROS (Robots Operating System) 最早是斯坦福大學的一個軟件框架，現在不管是工業機器人，還是娛樂用的機器人都運行著 ROS。 file:///C:\Users\Administrator.WIN-STED6B9V5UI\AppData\Local\Temp\ksohtml16216\wps9.png 一個機器人通常有很多個部件、傳感器，為了保證機器人不會因為某一個傳感器故障，導致整個系統癱瘓，所以采用了分布式的節點，利用不同節點之間的通訊收集傳感器數據和控制指令，這篇文檔后面會使用到的通訊協議就是 rosserial。和 ROS 連接的好處在于，一方面由 ROS 管理各個機器人節點更穩定，另一方面 ROS 現在已經有了非常多成熟的軟件包，使用 ROS 就可以非常方便的為自己的機器人添加攝像頭圖像識別、激光雷達建圖導航等高級功能。不過這篇文檔只會涉及 RT-Thread 和 ROS 建立基本的連接，實現小車的運動控制，之后可能會有后續文檔介紹如何連接激光雷達建圖，并進行全局路徑規劃。 這篇文章假定大家都已經會用 RT-Thread 的 env 工具下載軟件包，生成項目上傳固件到 stm32 上，并且熟悉 Ubuntu 的基本使用。 這里的開發環境搭建其實是需要搭建 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 1.3 啟動 ROS 啟動 ROS 的話我們需要確保它是常駐后臺運行的，所以我們可以使用 tmux： 1roscore 在 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 開發板連接，看起來差不多就是這樣。 file:///C:\Users\Administrator.WIN-STED6B9V5UI\AppData\Local\Temp\ksohtml16216\wps10.png 這里說明一下不同開發板的分工，STM32 運行著 RT-Thread 負責控制電機，接收傳感器信息；ARM 運行著 ROS 負責進行全局控制，例如給小車發出前進的指令。 2.1 RT-Thread 配置首先我們需要打開 usart2，因為 usart1 被 msh 使用了，保留作為調試還是挺方便的。file:///C:\Users\Administrator.WIN-STED6B9V5UI\AppData\Local\Temp\ksohtml16216\wps11.png 在 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 PWM 的配置： 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 下就可以看到有對應的配置了, file:///C:\Users\Administrator.WIN-STED6B9V5UI\AppData\Local\Temp\ksohtml16216\wps12.png file:///C:\Users\Administrator.WIN-STED6B9V5UI\AppData\Local\Temp\ksohtml16216\wps13.png 除此之外，我們還需要選擇 rosserial 軟件包： file:///C:\Users\Administrator.WIN-STED6B9V5UI\AppData\Local\Temp\ksohtml16216\wps14.png 可以看到上面默認的串口就是 USART2，這樣我們就可以生成對應的工程了： 1pkgs --update 2scons --target=mdk5 -s 如果我們打開 Keil 項目，首先需要把 main.c 修改為 main.cpp，因為 rosserial 很多數據格式的定義都是用 C++ 寫的，所以如果要使用 rosserial 庫，我們先得把后綴改為 cpp，這樣 Keil 就會用 C++ 編譯器編譯。 file:///C:\Users\Administrator.WIN-STED6B9V5UI\AppData\Local\Temp\ksohtml16216\wps15.png 下面是 main.cpp 的內容，其實就是初始化了電機，然后發布了 2 個話題 (topic)，一個是 /vel_x 告訴 ROS 當前小車的速度，一個是 /turn_bias 告訴 ROS 當前小車的旋轉速度。同時又訂閱了一個話題 /cmd_vel，用來接收從 ROS 發出的控制指令。代碼不是特別長，我也添加了一些注釋，所以這里就不一行行分析了。 1#include 2#include 3#include 4 5#include 6#include 7#include 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 27 28//Publisher 29std_msgs::Float64 velX_tmp; 30std_msgs::Float64 turnBias_tmp; 31ros:  ublisher xv("vel_x", &velX_tmp);32ros: ublisher 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} 另外還有對應的電機控制的代碼，不過這個大家的小車不同，驅動應當也不一樣，我這里由于小車電機上沒有編碼器，所以全部是開環控制的。 motors.h 1#include 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}; motors.c 1#include 2#include 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.3 ROS 控制小車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] 既然已經成功建立連接了，下一步就是寫小車控制的代碼了。我們先初始化一個工作區間： 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 2.4 參考文獻可以看到用 ROS 實現小車控制其實代碼量并不算多，只需要在自己小車原有的代碼上發布一些話題，告訴 ROS 小車當前的狀態，并且訂閱一個話題接收 ROS 的控制指令就可以了。 · 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 連接：file:///C:\Users\Administrator.WIN-STED6B9V5UI\AppData\Local\Temp\ksohtml16216\wps16.png 我們只需要在上一部分的 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 參考文獻· RT-Thread 使用 ESP8266 上網: · https://www.rt-thread.org/document/site/application-note/components/at/an0014-at-client/ 4 總結這里再總結一下，其實 RT-Thread 使用 rosserial 軟件包和 ROS 建立連接非常簡單，只需要在自己小車原有代碼的基礎上發布一些消息，告訴 ROS 小車當前的狀態，以及訂閱來自 ROS 的控制指令就可以了。 |
