【XR806开发板试用】+移植rosserial到XR806

1 XR806简介

板子来源于极术社区的试用，XR806的在线网址

其主要参数：

2 rosserial简介

官网

rosserial是用于非ROS设备与ROS设备进行通信的一种协议。它为非ROS设备的应用程序提供了ROS节点和服务的发布/订阅功能，使在非ROS环境中运行的应用能够通过串口或网络能够轻松地与ROS应用进行数据交互。

rosserial分为客户端和服务器两部分。rosserial客户端运行在运行在没有安装ROS的环境的应用中，通过串口或网络与运行在ROS环境中的rosserial服务器连接，并通过服务器节点在ROS中发布/订阅话题。

3 移植目标

通过rosserial使XR806能通过串口和TCP两种方式和ROS进行通信。

4 移植前准备

4.1 源码获取

[官方源码]( https://github.com/ros-drivers/rosserial.git
该仓库中的代码需要编译才能获取源码，为了直接获取源码，使用以下仓库的源码做为基础。

使用源码

该代码时属于RT-thread软件包，有较高的可信度。
clone下来的代码放在/ohosdemo/rosserial中，文件结构：
tree -L 1

tree -L 1

.
├── BUILD.gn
├── inc
├── port
└── src

1
2
3
4
5
6
7
8

• BUILD.gn ：配置文件
• inc ：rosserial源文件
• port：移植文件（为了和XR806适配的代码放在该文件下）

4.2 XR806 C++支持

rosserial为C++代码，需要XR806支持C++编译。

1. 修改/device/xradio/xr806/liteos_m/config.gni文件，添加以下内容：

board_cxx_flags = []

board_cxx_flags += SDK_cflags
board_cxx_flags += [
    "-includelog/log.h",
    "-DVIRTUAL_HCI",
    "-DCONFIG_ARM",
    #"-DNULL=((void*)0)",
    #"-std=c++17",
    "-lstdc++",
    "-fno-rtti",
    "-fno-exceptions"
   
]
1
2
3
4
5
6
7
8
9
10
11
12
13
14

大部分和board_cflags的配置一样，添加的编译项 "-lstdc++",-fno-rtti,-fno-exception是为了解决以下错误：

 undefined reference to `vtable for __cxxabiv1::__si_class_type_info'
1

该错误的原因是C++在链接时会有相关库链接不上
2. 在/ohosdemo/rosserial/BUILD.gn中添加以下代码：

cflags_cc = board_cxx_flags
1

表示支持c++编译

5 ROSserial移植核心

根据官网的移植介绍，只需要填写完以下模板即可：

class Hardware
{

  Hardware();

  // any initialization code necessary to use the serial port
  void init(); 

  // read a byte from the serial port. -1 = failure
  int read()

  // write data to the connection to ROS
  void write(uint8_t* data, int length);

  // returns milliseconds since start of program
  unsigned long time();

};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

• init()：提供初始化函数，初始化串口或者TCP网络
• read():读取一个字节
• write(uint8_t* data, int length):写字符
• time()：提供时间基准

6 串口通信

和串口相关的代码放在rosserial/port/UartHaedware.h
关键代码：

6.1 串口初始化：init()函数

void init()
    {
    //    HAL_Status status = HAL_ERROR;
      UART_InitParam param;
      param.baudRate = this->baudRate;

      param.dataBits = UART_DATA_BITS_8;
	    param.stopBits = UART_STOP_BITS_1;
	    param.parity = UART_PARITY_NONE;
	    param.isAutoHwFlowCtrl = 0;

       HAL_UART_Init(UARTID, &param);
    }
1
2
3
4
5
6
7
8
9
10
11
12
13

6.2 串口读取一个字节：read()函数

int read()
  {
      uint8_t rx_data;
      int32_t len=0;
      len = HAL_UART_Receive_IT(UARTID,&rx_data,1,1000);
      if(len>0)
      {
          return rx_data;
      }
      else return -1;
  }

1
2
3
4
5
6
7
8
9
10
11
12

6.3 串口写字节：write(uint8_t* data, int length)

// write data to the connection to ROS
  void write(uint8_t* data, int length)
  {
      HAL_UART_Transmit_IT(UARTID, data, length);
  }
1
2
3
4
5

6.4 时间基准：time()函数

  // returns milliseconds since start of program
  unsigned long time()
  {
    unsigned long temp = (unsigned long)OS_GetTime() * 1000;
      return temp;
  }
1
2
3
4
5
6

6.5 代码风格统一

为了保持ROS代码的编写风格一致，添加``rosserial/port/ros.h`
关键代码：

#define ROS_USE_TCP 0
#define ROS_USE_UART 1

namespace ros
{
  #if ROS_USE_TCP == 1
    typedef NodeHandle_<TCPHardware> NodeHandle;
  #endif

  #if ROS_USE_UART == 1
    typedef NodeHandle_<Hardware> NodeHandle;
  #endif
}
#endif
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

可以通过宏定义选择使用串口还是TCP

7 TCP通信

7.1 wifi连接

wifi连接使用官方在ohosdemo/wlan_demo中的代码。具体使用void wifi_device_event_test()函数和wifi连接成功后的回调函数:void Connected_deal(int state, WifiLinkedInfo *info)。

大致思路是在wifi连接线程中增加一个信号量，初始化信号量为0，rosserial线程会一直等待信号量有效后才会触发rosserial的相关函数，利用信号量作为两个线程间的同步，保证wifi顺利连接成功后才会进行rosserial的TCP通信。另外每次wifi重新连接后都会保证XR806会自动连接ROS。

信号量的三个关键函数：

• OS_Status OS_SemaphoreCreate(OS_Semaphore_t *sem, uint32_t initCount, uint32_t maxCount)初始化信号量，定义计数的初始值和最大值

• OS_Status OS_SemaphoreWait(OS_Semaphore_t *sem, OS_Time_t waitMS) 等待信号量有效，及信号量的计数不为0；等待时间可以使无限长，该参数为OS_WAIT_FOREVER

• OS_Status OS_SemaphoreRelease(OS_Semaphore_t *sem)释放信号量，信号量计数加1；

  主要代码：

  
  #define WIFI_DEVICE_CONNECT_AP_SSID "myyy"
  #define WIFI_DEVICE_CONNECT_AP_PSK "123456789"
  
  WifiEvent sta_event;
  
  void Connected_deal(int state, WifiLinkedInfo *info)
  {
  	if (state == WIFI_STATE_AVALIABLE) {
  		//释放信号量，计数加1
      	OS_SemaphoreRelease(&ros_sem);
  		// OS_Sleep(5);
          printf("\r\n======== Callback: connected========\r\n");
          
  	} 
  	else if (state == WIFI_STATE_NOT_AVALIABLE) {
  		printf("======== Callback: disconnected\n");
  	}
  }
  
  void wifi_device_event_test()
  {
  	const char ssid_want_connect[] = WIFI_DEVICE_CONNECT_AP_SSID;
  	const char psk[] = WIFI_DEVICE_CONNECT_AP_PSK;
  
      //创建信号量：初始值为0，最大值为2
      if(OS_SemaphoreCreate(&ros_sem, 0, 2) != OS_OK)
  	{
  		printf("\r\n sem creat fail!\r\n");
  		return ;
  	}
  
      sta_event.OnWifiConnectionChanged = Connected_deal;
      if (WIFI_SUCCESS != RegisterWifiEvent(&sta_event)) {
  		printf("Error: RegisterWifiEvent fail\n");
  		return;
  	}
  
  	printf("\n=========== Connect Test Start ===========\n");
  
  	if (WIFI_SUCCESS != EnableWifi()) {
  		printf("Error: EnableWifi fail.\n");
  		return;
  	}
  	printf("EnableWifi Success.\n");
  
  	if (WIFI_STA_ACTIVE == IsWifiActive())
  		printf("Wifi is active.\n");
  
  	OS_Sleep(1);
  	/*
  	...................
  	...................
  	...................
  	省略的代码 参见官方demo
  	
  	*/
  	
  	if (WIFI_SUCCESS != GetDeviceMacAddress(get_mac_res)) {
  		printf("Error: GetDeviceMacAddress Fail\n");
  		return;
  	}
  	printf("GetDeviceMacAddress Success.\n");
  	for (int j = 0; j < WIFI_MAC_LEN - 1; j++) {
  		printf("%02X:", get_mac_res[j]);
  	}
  	printf("%02X\n", get_mac_res[WIFI_MAC_LEN - 1]);
  
  }
  1
  2
  3
  4
  5
  6
  7
  8
  9
  10
  11
  12
  13
  14
  15
  16
  17
  18
  19
  20
  21
  22
  23
  24
  25
  26
  27
  28
  29
  30
  31
  32
  33
  34
  35
  36
  37
  38
  39
  40
  41
  42
  43
  44
  45
  46
  47
  48
  49
  50
  51
  52
  53
  54
  55
  56
  57
  58
  59
  60
  61
  62
  63
  64
  65
  66
  67
  68
  69

7.2 TCP客户端初始化

为了不和串口的Hardware类重名，类名为TCPHardware

 void init()
     {
         sock_fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
 
         // address info!
         struct sockaddr_in server_addr;
         memset(&server_addr, 0, sizeof(server_addr));
 
         server_addr.sin_family = AF_INET;
         server_addr.sin_port = htons(serverPort_);
         inet_pton(AF_INET, server_, &server_addr.sin_addr);
 
         // connect!
         if (connect(sock_fd, (sockaddr *)&server_addr, sizeof(server_addr)) < 0)
         {
             printf("connect tcp_server failed! \r\n");
             return;
         }
         printf("connect tcp_server successfuly! \r\n");
     }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

7.3 TCP读取字节

int read()
      {
          char ch[2];
          int bytes_received = recv(sock_fd, ch, 1, 0);
          if (bytes_received > 0)
          {
              return ch[0];
          }
          else
          {
              return -1;
          }
      }
  
1
2
3
4
5
6
7
8
9
10
11
12
13
14

7.4 TCP写字节

  void write(const uint8_t *data, int length)
      {
          send(sock_fd, data, length, 0);
      }
1
2
3
4

时间基准和6.4一样

8 测试

8.1 XR806 端

文件目录：

.
├── BUILD.gn
├── led_demo
│   ├── BUILD.gn
│   └── src
├── rosserial
│   ├── BUILD.gn
│   ├── inc
│   ├── port
│   └── src
└── wlan_demo
    ├── BUILD.gn
    ├── main.c
    ├── test_case.c
    └── test_case.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

其中：

• led_demo:led灯，指示作用
• rosserial:rosserialb包
• wlan_demo:负载wifi连接

8.1.1 配置文件BUILD.gn

# 必须，config中定义了头文件路径和关键宏定义
import("//device/xradio/xr806/liteos_m/config.gni")

# 必须，所有应用工程必须是app_打头
static_library("app_rosserial")
{
    configs = []
    sources = [
        "src/ros_helloworld.cpp",

        "inc/duration.cpp",
        "inc/time.cpp",
    ]
     #必须，board_cflags是在config.gni中定义的关键宏定义
     cflags = board_cflags
     # c++
     cflags_cc = board_cxx_flags

    #必须，board_include_dirs是在config.gni中定义的文件路径
    include_dirs = board_include_dirs

    # 根据实际情况添加头文件路径
   include_dirs += [
      "//kernel/liteos_m/kernel/arch/include",
      "./../wlan_demo/",
      "inc",
      "port" ,
      "//base/iot_hardware/peripheral/interfaces/kits",
      "//foundation/communication/wifi_lite/interfaces/wifiservice",
   ]
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

8.1.2 测试程序

• 编写一个发布话题：XR806_to_ROS

发布的内容为“hello world!”，时间间隔为1s

• 编写一个接收话题：ROS_to_XR860

接收的内容通过串口显示出来

#include <ros.h>
#include <std_msgs/String.h>

#include <stdio.h>
#include "ohos_init.h"
#include <stdlib.h>

//信号量的声明
extern OS_Semaphore_t ros_sem;

static OS_Thread_t g_main_thread;

static ros::NodeHandle  nh;
static std_msgs::String str_msg;
static ros::Publisher chatter("XR806_to_ROS", &str_msg);
static char hello_msg[25] = "hello world!";

// 回调函数
static void message_callback(const std_msgs::String& msgs)
{
    
    printf("\r\nresive:%s\r\n", msgs.data);
  
}

static ros::Subscriber<std_msgs::String> sub("ROS_to_XR860", &message_callback);


static void ROSThread(void *arg)
{   
    //等待信号量有效
     if (OS_SemaphoreWait(&ros_sem, OS_WAIT_FOREVER) == OS_OK)
     {
        printf("\r\n--------- star ROS----------------\r\n");
        nh.initNode();
        nh.advertise(chatter);
        nh.subscribe(sub);

        while (1)
        {
            if (nh.connected())
            {
                str_msg.data = hello_msg;
                chatter.publish(&str_msg); 
            }
            nh.spinOnce();
            OS_MSleep(1000);
        } 
     }   
}
void ROSMain(void)
{
    printf("\r\nROSserial Start\r\n");
    if (OS_ThreadCreate(&g_main_thread, "ROSThread", ROSThread, NULL,
			    OS_THREAD_PRIO_APP, 4 * 1024) != OS_OK) {
		printf("[ERR] Create MainThread Failed\n");
	}
}

SYS_RUN(ROSMain);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

8.1.3 编译程序

 hb build -f
1

可能会遇到下面问题：

这个flash分配的分配有问题：

cd 到 device/xradio/xr806/xr_skylark/project/demo/audio_demo/image/xr806

用文件image_auto_cal.cfg中的内容覆盖image_wlan_ble.cfg中的内容。

8.2 PC ROS端

Ubuntu版本：20版（18版也可以使用）

8.2.1 创建ros空间

mkdir -p rosworkspace/src
cd  rosworkspace
catkin_make
1
2
3

8.2.2 编写发布节点

import rospy
from std_msgs.msg import String

if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("talker_p")
    #3.实例化 发布者 对象
    pub = rospy.Publisher("ROS_to_XR860",String,queue_size=10)
    #4.组织被发布的数据，并编写逻辑发布数据
    msg = String()  #创建 msg 对象
    msg_front = "hello XR806    "
    count = 0  #计数器 
    # 设置循环频率
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():

        #拼接字符串
        msg.data = msg_front + str(count)

        pub.publish(msg)
        rate.sleep()
        #rospy.loginfo("写出的数据:%s",msg.data)
        count += 1
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

修改CMakeLists.txt

catkin_install_python(PROGRAMS
  scripts/test_pub.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
1
2
3
4

8.2.3 启动节点

1. 启动主节点：

roscore
1

2. 起动test_pub节点

source ./devel/setup.bash

rosrun ros_test test_pub.py 
1
2
3

3. 启用serial_node节点：

串口：

rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0 _baud:=115200
1

如果串口连接成功，终端显示：

INFO] [1640087588.757505]: ROS Serial Python Node
[INFO] [1640087588.762633]: Connecting to /dev/ttyUSB0 at 115200 baud
[INFO] [1640087591.079099]: Requesting topics...
[INFO] [1640087591.131236]: Note: publish buffer size is 512 bytes
[INFO] [1640087591.134777]: Setup publisher on XR806_to_ROS [std_msgs/String]
[INFO] [1640087591.142147]: Note: subscribe buffer size is 512 bytes
[INFO] [1640087591.144610]: Setup subscriber on ROS_to_XR860 [std_msgs/String]

1
2
3
4
5
6
7
8

注意查看串口的权限，如果权限不足，先开启权限：

# 查看权限
ll /dev/ttyUSB*
#开启权限
sudo chmod 777 /dev/ttyUSB*
1
2
3
4

TCP
默认节点11411

rosrun rosserial_python serial_node.py tcp
1

如果TCP连接成功，终端显示：

[INFO] [1640232707.758952]: ROS Serial Python Node
[INFO] [1640232707.763597]: Fork_server is: False
[INFO] [1640232707.764688]: Waiting for socket connections on port 11411
[INFO] [1640232707.765650]: Waiting for socket connection
[INFO] [1640232724.857086]: Established a socket connection from 192.168.43.49 on port 60872
[INFO] [1640232724.859268]: calling startSerialClient
[INFO] [1640232726.966374]: Requesting topics...
[INFO] [1640232727.289246]: Note: publish buffer size is 512 bytes
[INFO] [1640232727.290573]: Setup publisher on XR806_to_ROS [std_msgs/String]
[INFO] [1640232727.295084]: Note: subscribe buffer size is 512 bytes
[INFO] [1640232727.296294]: Setup subscriber on ROS_to_XR860 [std_msgs/String]
1
2
3
4
5
6
7
8
9
10
11

8.3 结果查询

1. ROS上查看话题：

 rostopic list 

/ROS_to_XR860
/XR806_to_ROS
/diagnostics
/rosout
/rosout_agg
1
2
3
4
5
6
7

可见 /ROS_to_XR860和/XR806_to_ROS两个话题

2. 查看/XR806_to_ROS

rostopic echo /ROS_to_XR860

data: "hello world!"
---
data: "hello world!"
---
data: "hello world!"
---
data: "hello world!"
---
data: "hello world!"
---
1
2
3
4
5
6
7
8
9
10
11
12

3. 查看XR806接收的 /ROS_to_XR860话题消息

串口连接

-------- star ROS----------------
resive:hello XR806    1
resive:hello XR806    2
resive:hello XR806    3
resive:hello XR806    4
resive:hello XR806    5
resive:hello XR806    6
resive:hello XR806    7
resive:hello XR806    8
resive:hello XR806    9
resive:hello XR806    10
resive:hello XR806    11
resive:hello XR806    12
1
2
3
4
5
6
7
8
9
10
11
12
13

TCP连接

--------- star ROS----------------
connect tcp_server successfuly! 
resive:hello XR806    1
resive:hello XR806    2
resive:hello XR806    3
resive:hello XR806    4
resive:hello XR806    5
resive:hello XR806    6
resive:hello XR806    7
resive:hello XR806    8
1
2
3
4
5
6
7
8
9
10