ROS using RT-Thread

introduction

This document mainly introduces how to use serial port or wireless to connect to ROS in RT-Thread, and will include the following contents:

• Part 1: ROS environment construction

• Part 2: RT-Thread rosserial package

• Part 2: RT-Thread adds USART2 and PWM

• Part 3: RT-Thread uses ESP8266 AT firmware to connect to the Internet

Here we first introduce what is ROS and why we need to connect to ROS?

The Robot Operating System ROS (Robots Operating System) was originally a software framework from Stanford University. Now both industrial robots and entertainment robots are running ROS.

img

A robot usually has many parts and sensors. In order to ensure that the robot does not fail due to a sensor failure, a distributed node is used to collect sensor data and control instructions through communication between different nodes. The communication protocol used later in this document is rosserial .

The advantage of connecting with ROS is that, on the one hand, it is more stable to manage each robot node by ROS, and on the other hand, ROS now has a lot of mature software packages. Using ROS, you can easily add advanced functions such as camera image recognition, lidar mapping and navigation to your robot.

However, this document will only cover the basic connection between RT-Thread and ROS to achieve motion control of the car. There may be subsequent documents to introduce how to connect to the lidar to build maps and perform global path planning.

This article assumes that everyone can use RT-Thread's env tool to download software packages, generate projects and upload firmware to stm32, and is familiar with the basic use of Ubuntu.

1 Introduction to ROS

1.1 ROS environment construction

The development environment here actually needs to be built in two parts, one is the ARM development board on the car (Raspberry Pi, NanoPi, etc.), and the other is your own computer, because we want to use the computer as a ROS slave node and connect it to the ROS master node on the car. However, the ROS installation of the development board and the computer is exactly the same.

Since we need to connect to ROS, we must first have a running ROS. Installing ROS is actually very simple. We recommend using Ubuntu 18 (Armbian is recommended for the development board) because the official support for Ubuntu is the highest priority. You can also refer to the official website for installation instructions .

Just enter the following 4 lines of commands to install ROS on Ubuntu.

sudo sh -c 'echo "deb https://mirror.tuna.tsinghua.edu.cn/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

sudo apt update

sudo apt install ros-melodic-ros-basecopymistakeCopy Success

I used the image source of Tsinghua University above, so that downloading ROS from China will be much faster, and I only installed the basic software package of ROS, and did not install the graphical software package gviz, gazebo, etc., because they were not used later.

1.2 ROS environment initialization

After ROS is installed, it needs to be initialized, but it only takes a few lines of commands:

sudo rosdep init
rosdep update

echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
source ~/.bashrccopymistakeCopy Success

1.3 Start ROS

If we start ROS, we need to make sure it runs in the background, so we can use tmux:

roscorecopymistakeCopy Success

After starting the ROS master node in tmux, we can exit it by pressing Ctrl + BD, and the ROS master node will still run in the background.

1.4 References

• Armbian

• ROS Melodic Installation

2 RT-Thread serial port connection to ROS

This section will introduce how to use the serial port to connect the STM32 development board running RT-Thread and the ARM development board running ROS. This is what it looks like.

img

Here we explain the division of labor of different development boards. STM32 runs RT-Thread to control the motor and receive sensor information; ARM runs ROS to perform global control, such as issuing forward instructions to the car.

2.1 RT-Thread Configuration

First we need to open usart2, because usart1 is used by msh, it is quite convenient to keep it for debugging.

img

In CubeMX, I turned on USART2 and also turned on 4-channel PWM, because I used 2 motors later, and each motor needed 2-channel PWM to control forward and backward respectively.

Next, you need to open the corresponding options in menuconfig. Considering that the default bsp of some development boards may not have these options, you can modify board/Kconfig and add the following content.

Serial port configuration:

menuconfig BSP_USING_UART
    bool "Enable UART"
    default y
    select RT_USING_SERIAL
    if BSP_USING_UART
        config BSP_USING_UART1
            bool "Enable UART1"
            default y

        config BSP_UART1_RX_USING_DMA
            bool "Enable UART1 RX DMA"
            depends on BSP_USING_UART1 && RT_SERIAL_USING_DMA
            default n

        config BSP_USING_UART2
            bool "Enable UART2"
            default y

        config BSP_UART2_RX_USING_DMA
            bool "Enable UART2 RX DMA"
            depends on BSP_USING_UART2 && RT_SERIAL_USING_DMA
            default n
    endifcopymistakeCopy Success

PWM configuration:

menuconfig BSP_USING_PWM
    bool "Enable pwm"
    default n
    select RT_USING_PWM
    if BSP_USING_PWM
    menuconfig BSP_USING_PWM3
        bool "Enable timer3 output pwm"
        default n
        if BSP_USING_PWM3
            config BSP_USING_PWM3_CH1
                bool "Enable PWM3 channel1"
                default n
            config BSP_USING_PWM3_CH2
                bool "Enable PWM3 channel2"
                default n
            config BSP_USING_PWM3_CH3
                bool "Enable PWM3 channel3"
                default n
            config BSP_USING_PWM3_CH4
                bool "Enable PWM3 channel4"
                default n
        endif
    endifcopymistakeCopy Success

In this way, we can see the corresponding configuration under env

img

img

In addition, we also need to select the rosserial package:

img

You can see that the default serial port above is USART2, so we can generate the corresponding project:

pkgs --update
scons --target=mdk5 -scopymistakeCopy Success

If we open the Keil project, we first need to change main.c to main.cpp, because many data format definitions of rosserial are written in C++, so if we want to use the rosserial library, we must first change the suffix to cpp, so that Keil will compile with the C++ compiler.

img

Below is the content of main.cpp, which actually initializes the motor and publishes two topics, one is /vel_x to tell ROS the current speed of the car, and the other is /turn_bias to tell ROS the current rotation speed of the car. At the same time, it subscribes to a topic /cmd_vel to receive control commands from ROS.

The code is not particularly long, and I have added some comments, so I will not analyze it line by line here.

#include <rtthread.h>
#include <rtdevice.h>
#include <board.h>

#include <ros.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Twist.h>
#include "motors.h"

ros::NodeHandle  nh;
MotorControl mtr(1, 2, 3, 4);   //Motor

bool msgRecieved = false;
float velX = 0, turnBias = 0;
char stat_log[200];

// 接收到命令时的回调函数
void velCB( const geometry_msgs::Twist& twist_msg)
{
  velX = twist_msg.linear.x;
  turnBias = twist_msg.angular.z;
  msgRecieved = true;
}

//Subscriber
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", velCB );

//Publisher
std_msgs::Float64 velX_tmp;
std_msgs::Float64 turnBias_tmp;
ros::Publisher xv("vel_x", &velX_tmp);
ros::Publisher xt("turn_bias", &turnBias_tmp);

static void rosserial_thread_entry(void *parameter)
{
    //Init motors, specif>y the respective motor pins
    mtr.initMotors();

    //Init node>
    nh.initNode();

    // 订阅了一个话题 /cmd_vel 接收控制指令
    nh.subscribe(sub);

    // 发布了一个话题 /vel_x 告诉 ROS 小车速度
    nh.advertise(xv);

    // 发布了一个话题 /turn_bias 告诉 ROS 小车的旋转角速度
    nh.advertise(xt);

    mtr.stopMotors();

    while (1)
    {
      // 如果接收到了控制指令
      if (msgRecieved)
      {
        velX *= mtr.maxSpd;
        mtr.moveBot(velX, turnBias);
        msgRecieved = false;
      }

      velX_tmp.data = velX;
      turnBias_tmp.data = turnBias/mtr.turnFactor;

      // 更新话题内容
      xv.publish( &velX_tmp );
      xt.publish( &turnBias_tmp );

      nh.spinOnce();
    }
}

int main(void)
{
    // 启动一个线程用来和 ROS 通信
    rt_thread_t thread = rt_thread_create("rosserial",     rosserial_thread_entry, RT_NULL, 2048, 8, 10);
    if(thread != RT_NULL)
    {
        rt_thread_startup(thread);
        rt_kprintf("[rosserial] New thread rosserial\n");
    }
    else
    {
        rt_kprintf("[rosserial] Failed to create thread rosserial\n");
    }
    return RT_EOK;
}copymistakeCopy Success

There is also the corresponding motor control code, but everyone's car is different, and the drive should also be different. Since there is no encoder on the car motor, all of them are open-loop controlled.

motors.h

#include <rtthread.h>

class MotorControl {
  public:
    //Var
    rt_uint32_t  maxSpd;
    float moveFactor;
    float turnFactor;

    MotorControl(int fl_for, int fl_back,
                 int fr_for, int fr_back);
    void initMotors();
    void rotateBot(int dir, float spd);
    void moveBot(float spd, float bias);
    void stopMotors();
  private:
    struct rt_device_pwm *pwm_dev;
    //The pins
    int fl_for;
    int fl_back;
    int fr_for;
    int fr_back;
    int bl_for;
    int bl_back;
    int br_for;
    int br_back;
};copymistakeCopy Success

motors.c

#include <rtthread.h>
#include <rtdevice.h>
#include "motors.h"

#define PWM_DEV_NAME "pwm3"

MotorControl::MotorControl(int fl_for, int fl_back,
                           int fr_for, int fr_back)
{
    this->maxSpd = 500000;
    this->moveFactor = 1.0;
    this->turnFactor = 3.0;

    this->fl_for = fl_for;
    this->fl_back = fl_back;

    this->fr_for = fr_for;
    this->fr_back = fr_back;
}

void MotorControl::initMotors() {
    /* 查找设备 */
    this->pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
    if (pwm_dev == RT_NULL)
    {
        rt_kprintf("pwm sample run failed! can't find %s device!\n", PWM_DEV_NAME);
    }
    rt_kprintf("pwm found %s device!\n", PWM_DEV_NAME);
    rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
    rt_pwm_enable(pwm_dev, fl_for);

    rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
    rt_pwm_enable(pwm_dev, fl_back);

    rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
    rt_pwm_enable(pwm_dev, fr_for);

    rt_pwm_set(pwm_dev, fr_back, maxSpd, 0);
    rt_pwm_enable(pwm_dev, fr_back);
}

// 小车运动
void MotorControl::moveBot(float spd, float bias) {
    float sL = spd * maxSpd;
    float sR = spd * maxSpd;
    int dir = (spd > 0) ? 1 : 0;

    if(bias != 0)
    {
        rotateBot((bias > 0) ? 1 : 0, bias);
        return;
    }

    if( sL < -moveFactor * maxSpd)
    {
        sL = -moveFactor * maxSpd;
    }
    if( sL > moveFactor * maxSpd)
    {
        sL = moveFactor * maxSpd;
    }

    if( sR < -moveFactor * maxSpd)
    {
        sR = -moveFactor * maxSpd;
    }
    if( sR > moveFactor * maxSpd)
    {
        sR = moveFactor * maxSpd;
    }

    if (sL < 0)
    {
        sL *= -1;
    }

    if (sR < 0)
    {
        sR *= -1;
    }

    rt_kprintf("Speed Left: %ld\n", (rt_int32_t)sL);
    rt_kprintf("Speed Right: %ld\n", (rt_int32_t)sR);

    if(dir)
    {
        rt_pwm_set(pwm_dev, fl_for, maxSpd, (rt_int32_t)sL);
        rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
        rt_pwm_set(pwm_dev, fr_for, maxSpd, (rt_int32_t)sR);
        rt_pwm_set(pwm_dev, fr_back, maxSpd, 0);
    }
    else
    {
        rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
        rt_pwm_set(pwm_dev, fl_back, maxSpd, (rt_int32_t)sL);
        rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
        rt_pwm_set(pwm_dev, fr_back, maxSpd, (rt_int32_t)sR);
    }

    rt_thread_mdelay(1);
}


// 小车旋转
void MotorControl::rotateBot(int dir, float spd) {
    float s = spd * maxSpd;
    if (dir < 0)
    {
        s *= -1;
    }
    if(dir)
    {
        // Clockwise
        rt_pwm_set(pwm_dev, fl_for, maxSpd, (rt_int32_t)s);
        rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
        rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
        rt_pwm_set(pwm_dev, fr_back, maxSpd, (rt_int32_t)s);
    }
    else
    {
        // Counter Clockwise
        rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
        rt_pwm_set(pwm_dev, fl_back, maxSpd, (rt_int32_t)s);
        rt_pwm_set(pwm_dev, fr_for, maxSpd, (rt_int32_t)s);
        rt_pwm_set(pwm_dev, fr_back, maxSpd, 0);
    }
    rt_thread_mdelay(1);
}

//Turn off both motors
void MotorControl::stopMotors()
{
    rt_pwm_set(pwm_dev, fl_for, maxSpd, 0);
    rt_pwm_set(pwm_dev, fl_back, maxSpd, 0);
    rt_pwm_set(pwm_dev, fr_for, maxSpd, 0);
    rt_pwm_set(pwm_dev, fr_back, maxSpd, 0);
}copymistakeCopy Success

Only this little bit of code is needed to connect to ROS, so ROS is not that mysterious. It is so popular because it is simple and easy to use.

Now that RT-Thread has been configured, the next step is to configure ROS.

2.2 ROS Configuration

After we upload the RT-Thread firmware to the board, we can use a USB-TTL to connect to the USART2 of the STM32 control board on one side and plug the other side into the USB port of the ARM control board. Then we can establish the connection and enter the command on the ARM board:

$ rosrun rosserial_python serial_node.py /dev/ttyUSB0copymistakeCopy Success

If you see the following output, the connection is successfully established:

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

2.3 ROS Controlled Car

Now that the connection has been successfully established, the next step is to write the code for controlling the car.

Let's first initialize a working range:

$ mkdir catkin_workspace && cd catkin_workspace
$ catkin_init_workspacecopymistakeCopy Success

Next, create a package:

$ cd src
$ catkin_create_pkg my_first_pkg rospycopymistakeCopy Success

This will automatically create a ROS package in the src directory.

We create a new file ros_cmd_vel_pub.py in the catkin_workspace/src/my_first_pkg/src directory:

#!/usr/bin/python

import rospy
from geometry_msgs.msg import Twist
from pynput.keyboard import Key, Listener

vel = Twist()
vel.linear.x = 0

def on_press(key):

    try:
        if(key.char == 'w'):
            print("Forward")
            vel.linear.x = 0.8
            vel.angular.z = 0

        if(key.char == 's'):
            print("Backward")
            vel.linear.x = -0.8
            vel.angular.z = 0

        if(key.char == 'a'):
            print("Counter Clockwise")
            vel.linear.x = 0
            vel.angular.z = -0.8

        if(key.char == 'd'):
            print("Clockwise")
            vel.linear.x = 0
            vel.angular.z = 0.8

        return False

    except AttributeError:
        print('special key {0} pressed'.format(key))
        return False

def on_release(key):
    vel.linear.x = 0
    vel.angular.z = 0

    return False

# Init Node
rospy.init_node('my_cmd_vel_publisher')
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)

# Set rate
rate = rospy.Rate(10)

listener = Listener(on_release=on_release, on_press = on_press)

while not rospy.is_shutdown():
    print(vel.linear.x)
    pub.publish(vel)
    vel.linear.x = 0
    vel.angular.z = 0
    rate.sleep()

    if not listener.running:
        listener = Listener(on_release=on_release, on_press = on_press)
        listener.start()copymistakeCopy Success

This is our Python control program. We can use the keyboard's wasd to control the car to move forward and backward, and rotate clockwise and counterclockwise. We need to add executable permissions to it:

$ chmod u+x ./ros_cmd_vel_pub.pycopymistakeCopy Success

This will compile the package in the catkin_worspace directory.

$ catkin_make
$ source devel/setup.bashcopymistakeCopy Success

We can finally start the program to control the car's movement from the computer:

rosrun my_first_pkg ros_cmd_vel_pub.pycopymistakeCopy Success

It can be seen that the amount of code to implement car control using ROS is not that much. You only need to publish some topics on the original code of your car, tell ROS the current status of the car, and subscribe to a topic to receive ROS control instructions.

2.4 References

• ros-pibot

3 RT-Thread Wireless Connection to ROS

3.1 rosserial configuration

In fact, wireless connection and wired connection are almost exactly the same, except that you first use ESP8266 to connect your control board to the Internet, and then use TCP connection to communicate with ROS. For tutorials on how to use ESP8266 to access the Internet with RT-Thread, please refer to the official website , which is very detailed and I will not repeat it here.

After ensuring that the development board has a network connection, we can configure it in rosserial to use a TCP connection:

img

We just need to add one line of code to main.cpp from the previous section:

// 设置 ROS 的 IP 端口号
nh.getHardware()->setConnection("192.168.1.210", 11411);

// 添加在节点初始化之前
nh.initNode();copymistakeCopy Success

The development board can communicate with ROS through TCP connection, which is very convenient.

3.2 ROS Configuration

Since we use TCP connection, we also need to start a server on ROS. Previously, we used the serial port to establish a connection, but now we use TCP:

$ rosrun rosserial_python serial_node.py tcpcopymistakeCopy Success

The rest of the code does not need to be changed at all, and we have now implemented a ROS wirelessly controlled car.

3.3 References

• RT-Thread uses ESP8266 to surf the Internet

4 Conclusion

To summarize here, it is actually very simple to establish a connection between RT-Thread and ROS using the rosserial software package. You only need to publish some messages based on the original code of your own car, tell ROS the current status of the car, and subscribe to the control instructions from ROS.