请选择 进入手机版 | 继续访问电脑版
搜索
查看: 2526|回复: 2

[分享] RT-Thread+STM32实现智能车目标识别系统的教程

[复制链接]

该用户从未签到

2586

主题

2613

帖子

0

蝴蝶豆

版主

最后登录
2021-3-16
发表于 2020-5-14 17:29:03 | 显示全部楼层 |阅读模式
引言

这篇文档主要介绍 RT-Thread 如何使用串口或者无线和 ROS 连接,会包含这么些内容:
第一部分:ROS 环境搭建
第二部分:RT-Thread rosserial 软件包
第二部分:RT-Thread 添加 USART2 和 PWM
第三部分:RT-Thread 使用 ESP8266 AT 固件联网

这里先介绍一下什么是 ROS?为什么要和 ROS 连接?
机器人操作系统 ROS (Robots Operating System) 最早是斯坦福大学的一个软件框架,现在不管是工业机器人,还是娱乐用的机器人都运行着 ROS。
微信图片_20200514172615.jpg
图片来源网络,如有侵权请联系删除
一个机器人通常有很多个部件、传感器,为了保证机器人不会因为某一个传感器故障,导致整个系统瘫痪,所以采用了分布式的节点,利用不同节点之间的通讯收集传感器数据和控制指令,这篇文档后面会使用到的通讯协议就是 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。
1 sudo sh -c 'echo "deb http://mirror.tuna.tsinghua.edu.cn/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

2 sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
3 sudo apt update
4 sudo apt install ros-melodic-ros-base

上面我使用了清华大学的镜像源,这样从国内下载 ROS 会快很多,而且我只安装了 ROS 的基本软件包,没有安装图形化软件包 gviz,gazebo 什么的,因为后面也没有用到。


1.2 ROS 环境初始化


ROS 安装好之后还需要进行初始化,不过也是只有短短几行命令:
1 sudo rosdep init

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


1.3 启动 ROS

启动 ROS 的话我们需要确保它是常驻后台运行的,所以我们可以使用 tmux:
1 roscore

在 tmux 里启动了 ROS 主节点后,我们就可以 Ctrl + B D 退出了,而 ROS 主节点依旧在后台运行。

1.4 参考文献2 RT-Thread 串口连接 ROS
这一部分会介绍如何使用串口将运行着 RT-Thread 的 STM32 开发板和运行着 ROS 的 ARM 开发板连接,看起来差不多就是这样。
微信图片_20200514172618.jpg
这里说明一下不同开发板的分工,STM32 运行着 RT-Thread 负责控制电机,接收传感器信息;ARM 运行着 ROS 负责进行全局控制,例如给小车发出前进的指令。

2.1 RT-Thread 配置


首先我们需要打开 usart2,因为 usart1 被 msh 使用了,保留作为调试还是挺方便的。
微信图片_20200514172621.png


在 CubeMX 里我打开了 USART2,另外还打开了 4 路 PWM,因为我后面使用了 2 个电机,每个电机需要 2 路 PWM 分别控制前进和后退。接下来还需要在 menuconfig 里面打开对应的选项,考虑到有的开发板默认的 bsp 可能没有这些选项,可以修改 board/Kconfig 添加下面的内容。


串口的配置:
1 menuconfig 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 的配置:
1 menuconfig 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 下就可以看到有对应的配置了,
微信图片_20200514172623.jpg
微信图片_20200514172626.jpg
除此之外,我们还需要选择 rosserial 软件包:
微信图片_20200514172629.jpg

可以看到上面默认的串口就是 USART2,这样我们就可以生成对应的工程了:
1pkgs --update
2scons --target=mdk5 -s



如果我们打开 Keil 项目,首先需要把 main.c 修改为 main.cpp,因为 rosserial 很多数据格式的定义都是用 C++ 写的,所以如果要使用 rosserial 库,我们先得把后缀改为 cpp,这样 Keil 就会用 C++ 编译器编译。
微信图片_20200514172632.jpg
下面是 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
10 ros::NodeHandle  nh;
11 MotorControl mtr(1, 2, 3, 4);   //Motor
12
13 bool msgRecieved = false;
14 float velX = 0, turnBias = 0;
15 char stat_log[200];
16
17 // 接收到命令时的回调函数
18 void 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
26 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", velCB );
27
28 //Publisher
29 std_msgs::Float64 velX_tmp;
30 std_msgs::Float64 turnBias_tmp;
31 ros:ublisher xv("vel_x", &velX_tmp);
32 ros:ublisher xt("turn_bias", &turnBias_tmp);
33
34 static 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
74 int 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 <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};


motors.c  
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

如果看到下面的输出,那就成功建立连接了:
1 tpl@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


我们终于就可以启动程序从电脑上控制小车运动了:
1 rosrun my_first_pkg ros_cmd_vel_pub.py


可以看到用 ROS 实现小车控制其实代码量并不算多,只需要在自己小车原有的代码上发布一些话题,告诉 ROS 小车当前的状态,并且订阅一个话题接收 ROS 的控制指令就可以了。


2.4 参考文献
3 RT-Thread 无线连接 ROS

3.1 rosserial 配置


其实无线连接和有线连接几乎是一模一样的,只不过是先用 ESP8266 使自己的控制板能连上网,然后用 tcp 连接和 ROS 通信,关于 RT-Thread 使用 ESP8266 上网的教程可以参照 官网:http://www.rt-thread.org/document/site/application-note/components/at/an0014-at-client/,非常详细了,我这里就不重复了。


确保开发板有网络连接后,我们就可以在 rosserial 里面配置为使用 tcp 连接:
微信图片_20200514172634.jpg

我们只需要在上一部分的 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 上网:
    http://www.rt-thread.org/document/site/application-note/components/at/an0014-at-client/

4 总结

这里再总结一下,其实 RT-Thread 使用 rosserial 软件包和 ROS 建立连接非常简单,只需要在自己小车原有代码的基础上发布一些消息,告诉 ROS 小车当前的状态,以及订阅来自 ROS 的控制指令就可以了。
回复

使用道具 举报

该用户从未签到

0

主题

364

帖子

0

蝴蝶豆

金牌会员

最后登录
2023-2-14
发表于 2020-5-14 19:08:18 | 显示全部楼层
厉害
回复

使用道具 举报

该用户从未签到

12

主题

1392

帖子

45

蝴蝶豆

金牌会员

最后登录
2021-8-25
发表于 2020-5-15 08:42:03 | 显示全部楼层
大佬门路挺广的哈
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 注册/登录

本版积分规则

关闭

站长推荐上一条 /3 下一条

Archiver|手机版|小黑屋|论坛-意法半导体STM32/STM8技术社区

GMT+8, 2024-3-28 22:25 , Processed in 1.168828 second(s), 34 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表