1 XR806簡介

板子來源于極術(shù)社區(qū)的試用，XR806的在線網(wǎng)址

其主要參數(shù)：

主控	XR806AF2L
DDR	SIP 288KB SRAM
存儲	SIP 160KB Code ROM. SIP 16Mbit Flash.
天線	板載WiFi/BT雙天線，可共存
按鍵	reboot按鍵 1，功能按鍵 1
燈	紅色電源指示燈 1，藍(lán)色可調(diào)節(jié)LED 1
供電	Type-C 5V
引腳	插針引腳 *9
調(diào)試方式	Type-C（已板載串口轉(zhuǎn)USB芯片）
晶振	外接40MHz晶振

2 rosserial簡介

官網(wǎng)

rosserial是用于非ROS設(shè)備與ROS設(shè)備進(jìn)行通信的一種協(xié)議。它為非ROS設(shè)備的應(yīng)用程序提供了ROS節(jié)點和服務(wù)的發(fā)布/訂閱功能，使在非ROS環(huán)境中運行的應(yīng)用能夠通過串口或網(wǎng)絡(luò)能夠輕松地與ROS應(yīng)用進(jìn)行數(shù)據(jù)交互。

rosserial分為客戶端和服務(wù)器兩部分。rosserial客戶端運行在運行在沒有安裝ROS的環(huán)境的應(yīng)用中，通過串口或網(wǎng)絡(luò)與運行在ROS環(huán)境中的rosserial服務(wù)器連接，并通過服務(wù)器節(jié)點在ROS中發(fā)布/訂閱話題。

3 移植目標(biāo)

通過rosserial使XR806能通過串口和TCP兩種方式和ROS進(jìn)行通信。

4 移植前準(zhǔn)備

4.1 源碼獲取

[官方源碼]( https://github.com/ros-drivers/rosserial.git
該倉庫中的代碼需要編譯才能獲取源碼，為了直接獲取源碼，使用以下倉庫的源碼做為基礎(chǔ)。

使用源碼

該代碼時屬于RT-thread軟件包，有較高的可信度。
clone下來的代碼放在/ohosdemo/rosserial中，文件結(jié)構(gòu)：
tree -L 1

tree -L 1.
├── BUILD.gn
├── inc
├── port
└── src

• BUILD.gn ：配置文件
• inc ：rosserial源文件
• port：移植文件（為了和XR806適配的代碼放在該文件下）

4.2 XR806 C++支持

rosserial為C++代碼，需要XR806支持C++編譯。

1. 修改/device/xradio/xr806/liteos_m/config.gni文件，添加以下內(nèi)容：

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"]

大部分和board_cflags的配置一樣，添加的編譯項 "-lstdc++",-fno-rtti,-fno-exception是為了解決以下錯誤：

 undefined reference to `vtable for __cxxabiv1::__si_class_type_info'

該錯誤的原因是C++在鏈接時會有相關(guān)庫鏈接不上
2. 在/ohosdemo/rosserial/BUILD.gn中添加以下代碼：

cflags_cc = board_cxx_flags

表示支持c++編譯

5 ROSserial移植核心

根據(jù)官網(wǎng)的移植介紹，只需要填寫完以下模板即可：

class Hardware
{Hardware();// any initialization code necessary to use the serial portvoid init(); // read a byte from the serial port. -1 = failureint read()// write data to the connection to ROSvoid write(uint8_t* data, int length);// returns milliseconds since start of programunsigned long time();};

• init()：提供初始化函數(shù)，初始化串口或者TCP網(wǎng)絡(luò)
• read():讀取一個字節(jié)
• write(uint8_t* data, int length):寫字符
• time()：提供時間基準(zhǔn)

6 串口通信

和串口相關(guān)的代碼放在rosserial/port/UartHaedware.h
關(guān)鍵代碼：

6.1 串口初始化：init()函數(shù)

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);}

6.2 串口讀取一個字節(jié)：read()函數(shù)

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;}

6.3 串口寫字節(jié)：write(uint8_t* data, int length)

// write data to the connection to ROSvoid write(uint8_t* data, int length){HAL_UART_Transmit_IT(UARTID, data, length);}

6.4 時間基準(zhǔn)：time()函數(shù)

  // returns milliseconds since start of programunsigned long time(){unsigned long temp = (unsigned long)OS_GetTime() * 1000;return temp;}

6.5 代碼風(fēng)格統(tǒng)一

為了保持ROS代碼的編寫風(fēng)格一致，添加``rosserial/port/ros.h`
關(guān)鍵代碼：

#define ROS_USE_TCP 0
#define ROS_USE_UART 1namespace ros
{#if ROS_USE_TCP == 1typedef NodeHandle_<TCPHardware> NodeHandle;#endif#if ROS_USE_UART == 1typedef NodeHandle_<Hardware> NodeHandle;#endif
}
#endif

可以通過宏定義選擇使用串口還是TCP

7 TCP通信

7.1 wifi連接

wifi連接使用官方在ohosdemo/wlan_demo中的代碼。具體使用void wifi_device_event_test()函數(shù)和wifi連接成功后的回調(diào)函數(shù):void Connected_deal(int state, WifiLinkedInfo *info)。

大致思路是在wifi連接線程中增加一個信號量，初始化信號量為0，rosserial線程會一直等待信號量有效后才會觸發(fā)rosserial的相關(guān)函數(shù)，利用信號量作為兩個線程間的同步，保證wifi順利連接成功后才會進(jìn)行rosserial的TCP通信。另外每次wifi重新連接后都會保證XR806會自動連接ROS。

信號量的三個關(guān)鍵函數(shù)：

• OS_Status OS_SemaphoreCreate(OS_Semaphore_t *sem, uint32_t initCount, uint32_t maxCount)初始化信號量，定義計數(shù)的初始值和最大值

• OS_Status OS_SemaphoreWait(OS_Semaphore_t *sem, OS_Time_t waitMS) 等待信號量有效，及信號量的計數(shù)不為0；等待時間可以使無限長，該參數(shù)為OS_WAIT_FOREVER

• OS_Status OS_SemaphoreRelease(OS_Semaphore_t *sem)釋放信號量，信號量計數(shù)加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) {//釋放信號量，計數(shù)加1OS_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;//創(chuàng)建信號量：初始值為0，最大值為2if(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]);}
  

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");}

7.3 TCP讀取字節(jié)

int read(){char ch[2];int bytes_received = recv(sock_fd, ch, 1, 0);if (bytes_received > 0){return ch[0];}else{return -1;}}

7.4 TCP寫字節(jié)

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

時間基準(zhǔn)和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

其中：

• led_demo:led燈，指示作用
• rosserial:rosserialb包
• wlan_demo:負(fù)載wifi連接

8.1.1 配置文件BUILD.gn

# 必須，config中定義了頭文件路徑和關(guān)鍵宏定義
import("//device/xradio/xr806/liteos_m/config.gni")# 必須，所有應(yīng)用工程必須是app_打頭
static_library("app_rosserial")
{configs = []sources = ["src/ros_helloworld.cpp","inc/duration.cpp","inc/time.cpp",]#必須，board_cflags是在config.gni中定義的關(guān)鍵宏定義cflags = board_cflags# c++cflags_cc = board_cxx_flags#必須，board_include_dirs是在config.gni中定義的文件路徑include_dirs = board_include_dirs# 根據(jù)實際情況添加頭文件路徑include_dirs += ["//kernel/liteos_m/kernel/arch/include","./../wlan_demo/","inc","port" ,"//base/iot_hardware/peripheral/interfaces/kits","//foundation/communication/wifi_lite/interfaces/wifiservice",]
}

8.1.2 測試程序

• 編寫一個發(fā)布話題：XR806_to_ROS

發(fā)布的內(nèi)容為“hello world!”，時間間隔為1s

• 編寫一個接收話題：ROS_to_XR860

接收的內(nèi)容通過串口顯示出來

#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!";// 回調(diào)函數(shù)
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);

8.1.3 編譯程序

 hb build -f

可能會遇到下面問題：

這個flash分配的分配有問題：

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

用文件image_auto_cal.cfg中的內(nèi)容覆蓋image_wlan_ble.cfg中的內(nèi)容。

8.2 PC ROS端

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

8.2.1 創(chuàng)建ros空間

mkdir -p rosworkspace/src
cd  rosworkspace
catkin_make

8.2.2 編寫發(fā)布節(jié)點

import rospy
from std_msgs.msg import Stringif __name__ == "__main__":#2.初始化 ROS 節(jié)點:命名(唯一)rospy.init_node("talker_p")#3.實例化 發(fā)布者 對象pub = rospy.Publisher("ROS_to_XR860",String,queue_size=10)#4.組織被發(fā)布的數(shù)據(jù)，并編寫邏輯發(fā)布數(shù)據(jù)msg = String()  #創(chuàng)建 msg 對象msg_front = "hello XR806    "count = 0  #計數(shù)器 # 設(shè)置循環(huán)頻率rate = rospy.Rate(1)while not rospy.is_shutdown():#拼接字符串msg.data = msg_front + str(count)pub.publish(msg)rate.sleep()#rospy.loginfo("寫出的數(shù)據(jù):%s",msg.data)count += 1

修改CMakeLists.txt

catkin_install_python(PROGRAMSscripts/test_pub.pyDESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

8.2.3 啟動節(jié)點

1. 啟動主節(jié)點：

roscore

2. 起動test_pub節(jié)點

source ./devel/setup.bashrosrun ros_test test_pub.py 

3. 啟用serial_node節(jié)點：

串口：

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

如果串口連接成功，終端顯示：

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]

注意查看串口的權(quán)限，如果權(quán)限不足，先開啟權(quán)限：

# 查看權(quán)限
ll /dev/ttyUSB*
#開啟權(quán)限
sudo chmod 777 /dev/ttyUSB*

TCP
默認(rèn)節(jié)點11411

rosrun rosserial_python serial_node.py tcp

如果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]

8.3 結(jié)果查詢

1. ROS上查看話題：

 rostopic list /ROS_to_XR860
/XR806_to_ROS
/diagnostics
/rosout
/rosout_agg

可見 /ROS_to_XR860和/XR806_to_ROS兩個話題

2. 查看/XR806_to_ROS

rostopic echo /ROS_to_XR860data: "hello world!"
---
data: "hello world!"
---
data: "hello world!"
---
data: "hello world!"
---
data: "hello world!"
---

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

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