RT-Thread如何使用串口或者无线和ROS连接

　　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 $（l***_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

　　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
　　2bool“Enable UART”
　　3defaulty
　　4select RT_USING_SERIAL
　　5ifBSP_USING_UART
　　6config BSP_USING_UART1
　　7bool“Enable UART1”
　　8defaulty
　　9
　　10config BSP_UART1_RX_USING_DMA
　　11bool“Enable UART1 RX DMA”
　　12depends on BSP_USING_UART1 && RT_SERIAL_USING_DMA
　　13defaultn
　　14
　　15config BSP_USING_UART2
　　16bool“Enable UART2”
　　17defaulty
　　18
　　19config BSP_UART2_RX_USING_DMA
　　20bool“Enable UART2 RX DMA”
　　21depends on BSP_USING_UART2 && RT_SERIAL_USING_DMA
　　22defaultn
　　23endif

PWM 的配置：
1menuconfig BSP_USING_PWM
2bool"Enable pwm"
3defaultn
4select RT_USING_PWM
5ifBSP_USING_PWM
6menuconfig BSP_USING_PWM3
7bool"Enable timer3 output pwm"
8defaultn
9ifBSP_USING_PWM3
10config BSP_USING_PWM3_CH1
11bool"Enable PWM3 channel1"
12defaultn
13config BSP_USING_PWM3_CH2
14bool"Enable PWM3 channel2"
15defaultn
16config BSP_USING_PWM3_CH3
17bool"Enable PWM3 channel3"
18defaultn
19config BSP_USING_PWM3_CH4
20bool"Enable PWM3 channel4"
21defaultn
22endif
23endif
这样我们在 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
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
13boolmsgRecieved = false;
14floatvelX = 0, turnBias = 0;
15charstat_log[ 200];
16
17// 接收到命令时的回调函数
18voidvelCB( constgeometry_msgs::Twist& twist_msg)
19{
20velX = twist_msg.linear.x;
21turnBias = twist_msg.angular.z;
22msgRecieved = true;
23}
24
25//Subscriber
26ros::Subscriber 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
34staticvoidrosserial_thread_entry( void*parameter)
35{
36//Init motors, specif>y the respective motor pins
37mtr.initMotors;
38
39//Init node>
40nh.initNode;
41
42// 订阅了一个话题 /cmd_vel 接收控制指令
43nh.subscribe(sub);
44
45// 发布了一个话题 /vel_x 告诉 ROS 小车速度
46nh.advertise(xv);
47
48// 发布了一个话题 /turn_bias 告诉 ROS 小车的旋转角速度
49nh.advertise(xt);
50
51mtr.stopMotors;
52
53while( 1)
54{
55// 如果接收到了控制指令
56if(msgRecieved)
57{
58velX *= mtr.maxSpd;
59mtr.moveBot(velX, turnBias);
60msgRecieved = false;
61}
62
63velX_tmp.data = velX;
64turnBias_tmp.data = turnBias/mtr.turnFactor;
65
66// 更新话题内容
67xv.publish( &velX_tmp );
68xt.publish( &turnBias_tmp );
69
70nh.spinOnce;
71}
72}
73
74intmain( void)
75{
76// 启动一个线程用来和 ROS 通信
77rt_thread_tthread = rt_thread_create( "rosserial", rosserial_thread_entry, RT_NULL, 2048, 8, 10);
78if(thread != RT_NULL)
79{
80rt_thread_startup(thread);
81rt_kprintf( "[rosserial] New thread rosserialn");
82}
83else
84{
85rt_kprintf( "[rosserial] Failed to create thread rosserialn");
86}
87returnRT_EOK;
88}
另外还有对应的电机控制的代码，不过这个大家的小车不同，驱动应当也不一样，我这里由于小车电机上没有编码器，所以全部是开环控制的。
motors.h
1# include
2
3classMotorControl{
4public:
5//Var
6rt_uint32_tmaxSpd;
7floatmoveFactor;
8floatturnFactor;
9
10MotorControl( intfl_for, intfl_back,
11intfr_for, intfr_back);
12voidinitMotors;
13voidrotateBot( intdir, floatspd) ;
14voidmoveBot( floatspd, floatbias) ;
15voidstopMotors;
16private:
17structrt_device_pwm* pwm_dev;
18//The pins
19intfl_for;
20intfl_back;
21intfr_for;
22intfr_back;
23intbl_for;
24intbl_back;
25intbr_for;
26intbr_back;
27};
motors.c
1# include
2# include
3# include"motors.h"
4
5# definePWM_DEV_NAME "pwm3"
6
7MotorControl::MotorControl( intfl_for, intfl_back,
8intfr_for, intfr_back)
9{
10this->maxSpd = 500000;
11this->moveFactor = 1.0;
12this->turnFactor = 3.0;
13
14this->fl_for = fl_for;
15this->fl_back = fl_back;
16
17this->fr_for = fr_for;
18this->fr_back = fr_back;
19}
20
21voidMotorControl::initMotors {
22/* 查找设备 */
23this->pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
24if(pwm_dev == RT_NULL)
25{
26rt_kprintf( "pwm sample run failed! can't find %s device!n", PWM_DEV_NAME);
27}
28rt_kprintf( "pwm found %s device!n", PWM_DEV_NAME);
29rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
30rt_pwm_enable(pwm_dev, fl_for);
31
32rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
33rt_pwm_enable(pwm_dev, fl_back);
34
35rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
36rt_pwm_enable(pwm_dev, fr_for);
37
38rt_pwm_set(pwm_dev, fr_back, maxSpd, 0);
39rt_pwm_enable(pwm_dev, fr_back);
40}
41
42// 小车运动
43voidMotorControl::moveBot( floatspd, floatbias) {
44floatsL = spd * maxSpd;
45floatsR = spd * maxSpd;
46intdir = (spd > 0) ? 1: 0;
47
48if(bias != 0)
49{
50rotateBot((bias > 0) ? 1: 0, bias);
51return;
52}
53
54if( sL < -moveFactor * maxSpd)
55{
56sL = -moveFactor * maxSpd;
57}
58if( sL > moveFactor * maxSpd)
59{
60sL = moveFactor * maxSpd;
61}
62
63if( sR < -moveFactor * maxSpd)
64{
65sR = -moveFactor * maxSpd;
66}
67if( sR > moveFactor * maxSpd)
68{
69sR = moveFactor * maxSpd;
70}
71
72if(sL < 0)
73{
74sL *= -1;
75}
76
77if(sR < 0)
78{
79sR *= -1;
80}
81
82rt_kprintf( "Speed Left: %ldn", ( rt_int32_t)sL);
83rt_kprintf( "Speed Right: %ldn", ( rt_int32_t)sR);
84
85if(dir)
86{
87rt_pwm_set(pwm_dev, fl_for, maxSpd, ( rt_int32_t)sL);
88rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
89rt_pwm_set(pwm_dev, fr_for, maxSpd, ( rt_int32_t)sR);
90rt_pwm_set(pwm_dev, fr_back, maxSpd, 0);
91}
92else
93{
94rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
95rt_pwm_set(pwm_dev, fl_back, maxSpd, ( rt_int32_t)sL);
96rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
97rt_pwm_set(pwm_dev, fr_back, maxSpd, ( rt_int32_t)sR);
98}
99
100rt_thread_mdelay( 1);
101}
102
103
104// 小车旋转
105voidMotorControl::rotateBot( intdir, floatspd) {
106floats = spd * maxSpd;
107if(dir < 0)
108{
109s *= -1;
110}
111if(dir)
112{
113// Clockwise
114rt_pwm_set(pwm_dev, fl_for, maxSpd, ( rt_int32_t)s);
115rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
116rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
117rt_pwm_set(pwm_dev, fr_back, maxSpd, ( rt_int32_t)s);
118}
119else
120{
121// Counter Clockwise
122rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
123rt_pwm_set(pwm_dev, fl_back, maxSpd, ( rt_int32_t)s);
124rt_pwm_set(pwm_dev, fr_for, maxSpd, ( rt_int32_t)s);
125rt_pwm_set(pwm_dev, fr_back, maxSpd, 0);
126}
127rt_thread_mdelay( 1);
128}
129
130//Turn off both motors
131voidMotorControl::stopMotors
132{
133rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
134rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
135rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
136rt_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 57600baud
4[INFO] [ 1567239476.425646]: Requesting topics...
5[INFO] [ 1567239476.464336]: Note: publish buffer size is 512bytes
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 512bytes
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
3importrospy
4from geometry_msgs.msg importTwist
5from pynput.keyboard importKey, Listener
6
7vel = Twist
8vel.linear.x = 0
9
10def on_press(key):
11
12try:
13if(key. char== 'w'):
14print( "Forward")
15vel.linear.x = 0.8
16vel.angular.z = 0
17
18if(key. char== 's'):
19print( "Backward")
20vel.linear.x = -0.8
21vel.angular.z = 0
22
23if(key. char== 'a'):
24print( "Counter Clockwise")
25vel.linear.x = 0
26vel.angular.z = -0.8
27
28if(key. char== 'd'):
29print( "Clockwise")
30vel.linear.x = 0
31vel.angular.z = 0.8
32
33returnFalse
34
35except AttributeError:
36print( 'special key {0} pressed'.format(key))
37returnFalse
38
39def on_release(key):
40vel.linear.x = 0
41vel.angular.z = 0
42
43returnFalse
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
54whilenotrospy.is_shutdown:
55print(vel.linear.x)
56pub.publish(vel)
57vel.linear.x = 0
58vel.angular.z = 0
59rate.sleep
60
61ifnotlistener.running:
62listener = Listener(on_release=on_release, on_press = on_press)
63listener.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
  

RT-Thread 无线连接 ROS
3.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 参考文献

• RT-Thread 使用 ESP8266 上网 : https://www.rt-thread.org/document/site/application-note/components/at/an0014-at-client/