XR806-移植rosserial-TCP通信

前面介绍了串口通信，下面介绍rosserial的TCP通信方式

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

}


  编写 rosserial 预留的Hardware类

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

  class TCPHardware
{
    char *server_;
    uint32_t serverPort_ = 11411;
    int sock_fd;

public:
    TCPHardware()
    {
        char ch[] = "192.168.31.195";
        server_ = (char *)malloc(20 * sizeof(char));
        if (server_ == NULL)
        {
            printf("\r\nserver_ fial!\r\n");
            return;
        }
        server_ = ch;
    }
    //自定义IP地址
    void set_connection(const char *url, int port = 11411)
    {


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

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

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

    unsigned long time()
    {
        unsigned long temp = (unsigned long)OS_GetTime() * 1000;
        return temp;
    }
};


  在ros.h函数中使用宏定义选择TCP还是串口通信

  #ifndef _ROS_H_
#define _ROS_H_

#include "ros/node_handle.h"
#include "UartHaedware.h"
#include "TCPHaedware.h"


#define ROS_USE_TCP 1

#define ROS_USE_UART 0

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

  #if ROS_USE_UART == 1
    typedef NodeHandle_<Hardware> NodeHandle;
  #endif
}
#endif


  测试

  XR806端

  编写一个发布者和订阅者。注意，该线程有信号量的限制

  #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 ROS!";

// 回调函数
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);


  PC 端

  roscore
source ./devel/setup.bash
rosrun ros_test test_pub.py 
rosrun rosserial_python serial_node.py tcp


  默认节点11411

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


• 启动serial_node节点：
• 启动test_pub节点
• 启动主节点：

连接成功后可以查看话题列表

查看/XR806_to_ROS

看到XR806订阅话题的信息

注意：必须先开PC上的ROS节点，启动服务器后才能启动RX806上的TCP 客户端。