Robot operating system (ro2) and RT thread communication

Time:2021-9-18

This article is published by RT thread official forum user @ wuhanstudio:https://club.rt-thread.org/as…, unauthorized forwarding is prohibited

MicroROS on RT-Thread

Recently, I just summarized the application of ros1 and ros2 on RT thread:
https://doc.wuhanstudio.cc/mi…

Robot operating system (ro2) and RT thread communication

preface

Introduction to ROS

At first, around 2007, Eric Berger and Keenan Wyrobek, two doctoral students in Stanford Robotics Laboratory, found that the students around them had a feeling of deterrence from robot development. Because the robot itself is an interdisciplinary subject, the students who do software do not know much about the mechanical structure and are not familiar with the design and assembly process of the robot; Algorithm students do not know much about embedded and the working principle of the underlying sensor driver, so they will encounter many obstacles in cooperation. In order to solve this problem, they designed the original robot operating system (ROS), which greatly improved the efficiency of team cooperation.

Because ROS is easy to use and quick to use, from the first edition of ROS released in 2007 and the first global ROS conf held to the fifth conference at the end of 2012, laboratories using ROS have spread all over the world. In 2013, ROS was taken over and maintained by the open source robotics Foundation (osrf). On the other hand, Stanford won the champion of DARPA driverless challenge in 2005, and then ROS was born in Stanford. ROS was soon widely used in the field of driverless. Moreover, the 2018 DARPA driverless challenge specially set up ROS track. The participating teams need to test their driverless vehicles in the ROS gazebo simulation environment and get scores.

Robot operating system (ro2) and RT thread communication

Although robot operating system (ROS) is named robot operating system, it is not an operating system, but a series of software packages developed on Linux. To illustrate the simplicity, efficiency and stability of ROS development, here are four important ROS designs:

  • Message: sometimes you may worry about what kind of data structure the sensor information should be sent out. Therefore, ROS defines the data formats of various common sensors. With a template, you only need to fill in the content, which saves the time of customizing the data structure. On the other hand, you can also write an. MSG file to define a new data structure, and ROS can automatically generate the corresponding header file. This is very similar to Google’s protobuf. Defining the data structure can automatically generate the source code. to make a long story short,A message of ROS is a packet that you want to send
  • Topic: after defining the data structure, how to send and receive data stably is also a problem. Simultaneous interpreting of one to one is relatively simple. But if multiple algorithms need to input the same sensor, and the same algorithm needs different sensor input, data synchronization will become very difficult soon. Therefore, ROS adopts the publish subscribe mode. For example, ROS sends image data with a predefined data structure. The published image information can be found under the topic / camera, and all nodes that subscribe to the topic will receive messages.It’s like we send the news to the newspaper, and all the people who subscribe to the newspaper will receive the news, so we just send the news, the remaining ROS will ensure that all nodes receive synchronization messages.

Robot operating system (ro2) and RT thread communication

  • Node: as mentioned earlier, after the message is published, ROS will ensure that all nodes subscribed to this topic will receive synchronous messages. thereforeThe function of a node is usually to receive some messages, do some processing, and maybe publish some new messages。 For example, a face detection node may subscribe to image related topics and tell other nodes the recognition results after recognition. Usually,A node does only one thingFor example, the node of face recognition only does face recognition and does not do fall detection at the same time. Such a node only handles a relatively small task, and then cooperates to achieve a larger goal. By the way,A node is not equal to a computer, because if a computer has strong performance, it can deploy several nodes, so as to make full use of resources.
  • Service: on the one hand, nodes can obtain the required information by subscribing to messages, on the other hand, nodes can also communicate. For example, a target tracking node may need to call the service of the target detection node to know where the target is before tracking. So service means literally. Each node can provide some services for other nodes to call.

Robot operating system (ro2) and RT thread communication

Therefore, with ROS, embedded students only need to publish sensor messages. Algorithm students subscribe to topics they need as algorithm input, and then publish the corresponding output. In this way, each student only needs to concentrate on maintaining his own node to ensure that the input and output of his own node are correct, and the whole system can operate normally.


In addition to software design, the ROS gazebo simulation environment mentioned above also greatly speeds up the iterative process. Before the processing and assembly of the real manipulator, the expected motion can be realized in the simulation environment, and unnecessary trial and error process is avoided.

Robot operating system (ro2) and RT thread communication

Of course, gazebo can also be connected with the actual robot for joint debugging [1]:

Robot operating system (ro2) and RT thread communication


RT thread and ROS

So the problem comes. As mentioned earlier, ROS is a software framework running on Linux. Linux itself is not a real-time system. Some tasks requiring high real-time performance of robots are not suitable for Linux. Although RT thread is a real-time operating system (RTOS), after all, the resources of MCU are limited and can not directly run a complete ROS.

Therefore, the common practice is to use the rich software packages of Linux to implement some top-level algorithms, and RT thread is responsible for real-time control related tasks. The communication between them is rosserial and micro, which will be introduced later_ ros。

Robot operating system (ro2) and RT thread communication

Rosserial and micro_ ROS difference

Their common goal is to connect MCU to ROS, enable MCU to communicate with ROS, and call ROS API on MCU,The main difference is that rosserial is for ros1, while micro_ ROS is for ros2 [2]。

After many years of development of the first generation ROS, of course, many unreasonable designs have been exposed. For example, when there are multiple ROS hosts, ROS needs to be set_ MASTER_ Uri and ROS_ HOST_ Name environment variable to help ROS master find IP addresses of different hosts. Once there are more hosts and the IP address is not too fixed, it will become very troublesome. It will be much more convenient if ROS can automatically find the host. In order to solve the historical problems of the first generation ROS and add new functions, osrf found that this would cause the new ROS to be destructively incompatible with the previous version, so they simply announced to enter the second generation ROS and redevelop a new set of ROS, that is, ros2.

The new generation of ros2 uses data distribution service (DDS) communication to automatically discover the host, which makes the distributed system design more convenient. However, DDS is not a lightweight framework. If MCU is to access DDS, it must need sufficient hardware resources. However, ros2 is mainly aimed at 64 bit medium and high-end platforms, and MCU is not the main support object of ros2, so that the first generation of rosserial basically gives up maintenance to the second generation. Nevertheless, many developers hope to connect MCU to ros2 and call the API of ros2 directly on MCU, so micro ROS is finally available. Although micro ROS realizes lightweight DDS with micro xrce-dds, butAt least a 32-bit midrange MCU is required, unlike the first generation of rosserial, it can run on 8-bit low-end MCU.

RT thread control kobuki robot

Ros1 / ros2, rosserial (ros1) and micro were introduced earlier_ ROS (ros2). Now RT thread has rosserial and micro_ ROS software package can communicate with ros1 / ros2 respectively. Meanwhile, there are also kobuki robot chassis software package [5] and lidar rplidar software package [6]. It should be possible to skip raspberry pie and directly use RT thread to make a slam robot. By the way,Rosserial supports serial port and TCP communication, micro_ ROS supports serial port and UDP communication; Rosserial only supports C + +, while micro_ ROS supports C / C + +.

<img width=50%>

Taking RT thread cooperating with ROS to control kobuki robot as an example, this paper introduces how to communicate with ros1 (rosserial) and ros2 (micro_ros) on RT thread, and the following examples take wireless (WiFi) as an example, so that the MCU running RT thread can directly access ROS without raspberry pie.By the way, as long as the following operations ensure that there are three serial ports, you can use any development board.Of course, if you don’t need RTT console, you only need 2.

Robot operating system (ro2) and RT thread communication

RT thread networking using esp8266

Whether rosserial (ros1) or micro_ ROS (ros2), the networking part is the same. Here, take the at firmware networking of esp8266 as an example. Of course, Ethernet can also be used.

Before starting RT thread development, it is necessary to ensure that the esp8266 used for networking is at firmware, which can be downloaded here (AI thinker_esp8266_at_firmware_dout_v1.5.4.1-a_20171130. RAR):

https://docs.ai-thinker.com/e…

After connecting the serial port of the esp8266 to the computer, pull down the gpio0 to reset and enter the firmware download mode, so that the esp8266 can process the at command:

$ esptool.exe --port com5 erase_flash
$ esptool.exe --port com5 write_flash --flash_mode dout 0 Ai-Thinker_ESP8266_DOUT_8Mbit_v1.5.4.1-a_20171130.bin

After connecting the esp8266 to your own development board serial port (UART2), create a new project in RT thread studio according to your own development board model. As mentioned earlier, we need three serial ports, so we need to open three serial ports in driver / board. H, one is used as a console, one is connected to esp8266, and the last one communicates with kobuki robot:

# MSH Console
#define BSP_USING_UART1
#define BSP_UART1_TX_PIN       "PA9"
#define BSP_UART1_RX_PIN       "PA10"

# ESP8266
#define BSP_USING_UART2
#define BSP_UART2_TX_PIN       "PA2"
#define BSP_UART2_RX_PIN       "PA3"

# Kobuki Robot
#define BSP_USING_UART3
#define BSP_UART3_TX_PIN       "PB10"
#define BSP_UART3_RX_PIN       "PB11"

Next, add at in RT thread studio_ Device package:

Robot operating system (ro2) and RT thread communication

Double click the software package to modify the configuration options, select your own WiFi module, configure WiFi SSID and password, and select UART2 as the communication interface:

Robot operating system (ro2) and RT thread communication

Save the configuration and compile. If everything is normal, the RT thread will automatically connect to WiFi after power on, and can ping the external host:

 \ | /
- RT -     Thread Operating System
 / | \     4.0.2 build Jun 14 2021
 2006 - 2019 Copyright by rt-thread team
[I/sal.skt] Socket Abstraction Layer initialize success.
[I/at.clnt] AT client(V1.3.0) on device uart2 initialize success.
msh >[I/at.dev.esp] esp0 device wifi is connected.
[I/at.dev.esp] esp0 device network initialize successfully.
[E/at.clnt] execute command (AT+CIPDNS_CUR?) failed!
[W/at.dev.esp] please check and update esp0 device firmware to support the "AT+CIPDNS_CUR?" cmd.
[E/at.clnt] execute command (AT+CIPDNS_CUR?) failed!
[W/at.dev.esp] please check and update esp0 device firmware to support the "AT+CIPDNS_CUR?" cmd.
[E/at.skt] AT socket (0) receive timeout (2000)!

msh >ping rt-thread.org
32 bytes from 118.31.15.152 icmp_seq=0 time=242 ms
32 bytes from 118.31.15.152 icmp_seq=1 time=245 ms
32 bytes from 118.31.15.152 icmp_seq=2 time=241 ms
32 bytes from 118.31.15.152 icmp_seq=3 time=245 ms

msh >

Of course, you can also refer to the documents on the RT thread official website for networking operations:

https://www.rt-thread.org/doc…

Finally, we connect the serial port of kobuki robot to the serial port (uart3) of RT thread development board, and add kobuki in RTT Studio software package:

Robot operating system (ro2) and RT thread communication

Double click the software package and configure the kobuki communication interface as uart3 to prepare for docking with ROS.

Robot operating system (ro2) and RT thread communication

ROS1 (rosserial)

The first generation ROS supports serial port and TCP communication. Taking TCP as an example, add the software package in RT thread Studio:

Robot operating system (ro2) and RT thread communication

After double clicking the software package, configure the routine of using TCP connection and adding kobuki robot:

Robot operating system (ro2) and RT thread communication

The complete code is available on GitHub, so the core initialization code of rosserial is mainly explained here. Initialization on MCU as like as two peas on PC is almost the same.setConnection()Define the IP address and port of the ROS master. NextinitNode()Initialize the node and finally subscribe/cmd_velIt is used to receive control information from the computer and set the speed of kobuki.

static ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", messageCb );

static void rosserial_kobuki_thread_entry(void *parameter)
{
    // Please make sure you have network connection first
    // Set ip address and port
    nh.getHardware()->setConnection("192.168.199.100", 11411);

    nh.initNode();
    nh.subscribe(sub);
    while (1)
    {
        nh.spinOnce();
        rt_thread_mdelay(500);
    }
}

Once receiving the control information from PC, update the linear motion speed and rotation angular speed in the callback function:

static void messageCb( const geometry_msgs::Twist& twist_msg)
{
    linear_x  = twist_msg.linear.x;
    angular_z = twist_msg.angular.z;
}

Of course, the kobuki thread keeps sending control commands becauseThe characteristic of kobuki chassis is that once no command is received for a period of time, the robot will stop movingSo we need to keep sending control commands.

static void kobuki_entry(void *parameter)
{
    kobuki_init(&robot);
    while(1)
    {
        rt_thread_mdelay(100);
        kobuki_set_speed(linear_x, angular_z);
    }
    kobuki_close(&robot);
}

Of course, since we are remote control, it is certainly impossible to connect the serial port to the RTT console to input commands to start the robot, so we need to automatically start the task in main.cpp.

#include <rtthread.h>
#include <ros.h>

int main(void)
{
    extern int rosserial_kobuki_control(int argc, char **argv);
    rt_thread_mdelay(10000);
    rosserial_kobuki_control(0, 0);
}

Note that the main function here ismain.cppBecauseRosserial only supports C++。 After counting, the code related to rosserial is less than 10 lines, so it also reflects the simplicity and ease of use of ROS. Finally,There was a 1Hz bug in rosserial that caused the system to respond very slowly. Thanks to zhengyangliu’s PR, it has been fixed, the overall rosserial use of serial port and TCP is very stable (however, esp8266 itself is not very stable).

The RT thread related code is over. Of course, we also need to start the ROS master node. Here I use the docker image to map the required ports:

$ docker run -p 11411:11411 -it helloysd/rosserial:noetic /bin/bash

stayBash of docker containerInstall relevant software packages and start ROS serial:

$ apt update
$ apt install curl
$ curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
$ apt update
$ apt install ros-noetic-teleop-twist-keyboard ros-noetic-rosserial-python
$ rosrun rosserial_python serial_node.py tcp

If everything goes well, you will see the RTT node connected:

[email protected]:/# rosrun rosserial_python serial_node.py tcp
[INFO] [1624089042.689792]: ROS Serial Python Node
[INFO] [1624089042.693095]: Fork_server is: False
[INFO] [1624089042.693884]: Waiting for socket connections on port 11411
[INFO] [1624089042.694750]: Waiting for socket connection
[INFO] [1624089055.564072]: Established a socket connection from 172.17.0.1 on port 55784
[INFO] [1624089055.565926]: calling startSerialClient
[INFO] [1624089057.674819]: Requesting topics...
[INFO] [1624089229.750517]: Note: subscribe buffer size is 512 bytes
[INFO] [1624089229.751418]: Setup subscriber on cmd_vel [geometry_msgs/Twist]

Finally, you can use the teleop software package of ROS to remotely control the kobuki robot with a keyboard:

$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

Robot operating system (ro2) and RT thread communication

ROS2 (micro_ros)

As mentioned earlier, the communication between the second generation ROS hosts is based on DDS / rtps, but these do not need to be implemented by themselves. You can directly add micro in RT thread studio_ ROS package:

Robot operating system (ro2) and RT thread communication

Almost the same process. Double click the software package and select UDP as the communication mode. Select different architectures according to your own development board, because micro_ ROS related libraries are pre compiled as libmicros. A, and now support cortex M0, cortex m3, cortex M4 and cortex M7. If it is other architectures, it needs to be in extras / library_ Add relevant compiled files under generation and recompile the library files of the corresponding schema. On the other hand, the default compilation library file uses GCC 5.4.1, which is consistent with the compiler version of RTT studio and is also the most commonly used version of embedded. If it is an updated version of GCC or keil and other compilers, libmicros. A also needs to be regenerated.

Of course, after selecting kobuki’s routine, we also need to specify kobuki’s serial port uart3.

Robot operating system (ro2) and RT thread communication

Similarly, the routine exports relevant tasks as commands by default. It is impossible for us to connect the serial port to start tasks under the RTT console every time after the robot is powered on, so we need to automatically start relevant tasks in main. C:

#include <rtthread.h>

int main(void)
{
    extern int microros_kobuki_control(int argc, char **argv);
    rt_thread_mdelay(10000);
    microros_kobuki_control(0, (void*)0);
}

Here is an introduction to micro_ To start the process of ROS, set micro first_ The IP address and UPD port number of the ROS client. After starting the registration of relevant nodes, subscribe to the topic you need, start the executor, and set the relevant callback function.

set_microros_udp_transports("192.168.199.100", 9999);

allocator = rcl_get_default_allocator();

// create init_options
if (rclc_support_init(&support, 0, NULL, &allocator) != RCL_RET_OK)
{
    rt_kprintf("[micro_ros] failed to initialize\n");
    return;
};

// create node
if (rclc_node_init_default(&node, "micro_ros_rtt_sub_twist", "", &support) != RCL_RET_OK)
{
    rt_kprintf("[micro_ros] failed to create node\n");
    return;
}

// create subscriber
rclc_subscription_init_default(
  &subscriber,
  &node,
  ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
  "cmd_vel");

// create executor
rclc_executor_init(&executor, &support.context, 1, &allocator);
rclc_executor_add_subscription(&executor, &subscriber, &msg, &kobuki_callback, ON_NEW_DATA);

Whenever a new message is received, the control information of the robot will be updated in the callback function.

//twist message cb
static void kobuki_callback(const void *msgin) {
    const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin;
    linear_x = msg->linear.x;
    angular_z = msg->angular.z;
}

Similarly, the kobuki robot needs to keep sending control commands. Once the control command is not received for a period of time, kobuki will automatically stop, which is also a protection mechanism.

static void kobuki_entry(void *parameter)
{
    kobuki_init(&robot);
    while(1)
    {
        rt_thread_mdelay(100);
        kobuki_set_speed(linear_x, angular_z);
    }
    kobuki_close(&robot);
}

static void microros_kobuki_entry(void *parameter)
{
    while(1)
    {
        rt_thread_mdelay(100);
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
    }
}

After the RT thread development is completed, of course, you still need to start the micro on the computer_ ROS client, I still use docker here:

$ docker run -it -p 9999:9999/udp --privileged microros/micro-ros-agent:foxy udp4 -p 9999

After starting the client, we can connect the RTT:

 \ | /
- RT -     Thread Operating System
 / | \     4.0.4 build Jun  9 2021
 2006 - 2021 Copyright by rt-thread team
msh >
msh >microros_pub_int32
[micro_ros] node created
[micro_ros] publisher created
[micro_ros] timer created
[micro_ros] executor created
[micro_ros] New thread mr_pubint32

If everything is normal, we can see the connection information of the client in the output of the client:

[1623057529.937043] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 4
[1623057529.937150] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1623057541.764331] info     | Root.cpp           | create_client            | create                 | client_key: 0x6F7B427A, session_id: 0x81
[1623057541.764507] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x1870348922, address: 0

Finally, of course, start teleop to control the kobuki robot with the keyboard:

$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

summary

In general, it has to be said that the rosserial initialization process of ros1 is simpler.

On the other hand, if you want to run ROS master node directly on RT thread, you should also expect RT thread smart.

References

[1] Gazebo simulator:https://www.youtube.com/watch…
[2] Difference between microros and rosseiral:https://micro.ros.org/docs/co…
[3] Rosserial package:https://github.com/wuhanstudi…
[4] micro_ ROS package:https://github.com/wuhanstudi…
[5] Kobuki package:https://github.com/wuhanstudi…
[6] Rplidar package:https://github.com/wuhanstudi…