pyRBLidar/rb_lidar.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <pthread.h>

#include "rb_lidar.h"

#define BUF_SIZE 1206
// UDP接收线程
void* udp_listener(void* arg) {
    RBLidar* lidar = (RBLidar*)arg;
    int sockfd;
    struct sockaddr_in server_addr, client_addr;
    socklen_t addr_len = sizeof(client_addr);
    udp_packet_t udp_packet_temp;

    // 创建UDP套接字
    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if (sockfd < 0) {
        perror("socket");
        return NULL;
    }

    // 设置服务器地址
    memset(&server_addr, 0, sizeof(server_addr));
    server_addr.sin_family = AF_INET;
    inet_pton(AF_INET, lidar->ip, &server_addr.sin_addr);
    server_addr.sin_port = htons(lidar->port);

    // 绑定套接字
    if (bind(sockfd, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
        perror("bind");
        close(sockfd);
        return NULL;
    }

    printf("Listening for UDP packets on %s:%d...\n", lidar->ip, lidar->port);

    // 接收数据
    while (1) {
        int len = recvfrom(sockfd, (char *)&udp_packet_temp, BUF_SIZE, 0, (struct sockaddr*)&client_addr, &addr_len);
        if (len < 0) {
            perror("recvfrom");
            continue;
        }

        // 使用双缓冲机制
        pthread_mutex_lock(&lidar->mutex);

        // 将接收到的UDP包中的子包存入当前缓冲区
        for (int i = 0; i < CONFIG_UDP_BLOCKS; i++) {
            sub_packet_t* sub_packet = &udp_packet_temp.sub_packet[i];
            
            // 将子包存入当前缓冲区
            memcpy(&lidar->buffer[lidar->current][lidar->valid_count[lidar->current]], sub_packet, sizeof(sub_packet_t));

            // 统计有效数据
            lidar->valid_count[lidar->current]++;

            // 检查azimuth是否为0
            if (sub_packet->azimuth == 0) {
                // 切换到下一个缓冲区并通知处理线程
                lidar->current = (lidar->current + 1) % 2; // 切换缓冲区

                lidar->valid_count[lidar->current] = 0;
                memcpy(&lidar->buffer[lidar->current][lidar->valid_count[lidar->current]], sub_packet, sizeof(sub_packet_t));
                lidar->valid_count[lidar->current]++;

                // pthread_cond_signal(&lidar->cond); // 通知处理线程
            }
        }

        pthread_mutex_unlock(&lidar->mutex);
    }

    close(sockfd);
    return NULL;
}

// 组包线程
void* packet_processor(void* arg) {
    RBLidar* lidar = (RBLidar*)arg;

    // 处理数据
    while (1) {
        pthread_mutex_lock(&lidar->mutex);
        pthread_cond_wait(&lidar->cond, &lidar->mutex); // 等待数据

        // 处理当前缓冲区数据
        int next_index = (lidar->current + 1) % 2; // 获取上一缓冲区索引
        int count = lidar->valid_count[next_index]; // 获取有效数据的数量

        // 创建临时缓冲区
        sub_packet_t* temp_buffer = (sub_packet_t*)malloc(sizeof(sub_packet_t) * SUBPKT_BUF_COUNT);
        int temp_count = 0;

        // 处理当前缓冲区中的每个有效子包
        for (int i = 0; i < count; i++) {
            sub_packet_t* sub_packet = &lidar->buffer[next_index][i];
            
            memcpy(&temp_buffer[temp_count++], sub_packet, sizeof(sub_packet_t));
        }

        // 调用回调函数
        lidar->callback((const char*)temp_buffer, sizeof(sub_packet_t) * temp_count);

        // 重置当前缓冲区
        memset(lidar->buffer[next_index], 0, sizeof(sub_packet_t) * SUBPKT_BUF_COUNT);
        lidar->valid_count[next_index] = 0; // 重置有效数据计数

        free(temp_buffer); // 释放临时缓冲区

        pthread_mutex_unlock(&lidar->mutex);
    }
    return NULL;
}



// 创建RBLidar实例
RBLidar* rblidar_create(const char* ip, int port, callback_t callback) {
    RBLidar* lidar = (RBLidar*)malloc(sizeof(RBLidar));
    lidar->ip = strdup(ip);
    lidar->port = port;
    lidar->callback = callback;

    // 初始化双缓冲区和其他线程同步机制
    for (int i = 0; i < 2; i++) {
        lidar->buffer[i] = (sub_packet_t*)malloc(sizeof(sub_packet_t) * SUBPKT_BUF_COUNT);
    }
    lidar->current = 0;
    pthread_mutex_init(&lidar->mutex, NULL);
    pthread_cond_init(&lidar->cond, NULL);

    // 创建接收线程
    pthread_t listener_thread;
    pthread_create(&listener_thread, NULL, udp_listener, lidar);
    pthread_detach(listener_thread);  // 让线程在结束时自动回收资源

    // 创建组包线程
    pthread_t processor_thread;
    pthread_create(&processor_thread, NULL, packet_processor, lidar);
    pthread_detach(processor_thread);

    return lidar;
}

// 释放RBLidar实例
void rblidar_destroy(RBLidar* lidar) {
    for (int i = 0; i < 2; i++) {
        free(lidar->buffer[i]);
    }
    pthread_mutex_destroy(&lidar->mutex);
    pthread_cond_destroy(&lidar->cond);
    free(lidar->ip);
    free(lidar);
}