Measure/Modules/DataToCloud/lidar_manager.cpp

#include "lidar_manager.h"

#define VIEWER 0

CloudDataManager::CloudDataManager() = default;

CloudDataManager::~CloudDataManager() = default;

void CloudDataManager::receiveClient() {
//    172 120
//    250 104
    RslidarMqttAsyncClient client;
    client.init();
    google::protobuf::RepeatedPtrField<SubscribeEtc> subs = client.getSubscribeEtc();
    int view_port[4];
    std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds_xyz;
    std::map<std::string, pcl::PointCloud<pcl::PointXYZI>::Ptr> clouds_xyzi;
    for (const auto &sub: subs) {
        clouds_xyz.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr>(sub.topic(),
                                                                                      new pcl::PointCloud<pcl::PointXYZ>));
        clouds_xyzi.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZI>::Ptr>(sub.topic(),
                                                                                        new pcl::PointCloud<pcl::PointXYZI>));
    }
    pcl::visualization::PCLVisualizer viewer("viewer_0");
    viewer.createViewPort(0.0, 0.5, 0.5, 1, view_port[0]);
    viewer.createViewPort(0.5, 0.5, 1.0, 1, view_port[1]);
    viewer.createViewPort(0.0, 0, 0.5, 0.5, view_port[2]);
    viewer.createViewPort(0.5, 0, 1.0, 0.5, view_port[3]);
    viewer.addText("view_port_1", 10, 10, 20, 0, 1, 0, "view_port_1", view_port[0]);
    viewer.addText("view_port_2", 10, 10, 20, 0, 1, 0, "view_port_2", view_port[1]);
    viewer.addText("view_port_3", 10, 10, 20, 0, 1, 0, "view_port_3", view_port[2]);
    viewer.addText("view_port_4", 10, 10, 20, 0, 1, 0, "view_port_4", view_port[3]);
    viewer.addCoordinateSystem(1, "v1_axis", view_port[0]);
    viewer.addCoordinateSystem(1, "v2_axis", view_port[1]);
    viewer.addCoordinateSystem(1, "v3_axis", view_port[2]);
    viewer.addCoordinateSystem(1, "v4_axis", view_port[3]);
    sleep(3);
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr zx_points(new pcl::PointCloud<pcl::PointXYZ>);
    for (auto iter = clouds_xyz.begin(); iter != clouds_xyz.end(); iter++) {
        client.getCloudData(iter->first, iter->second);
        zx_points->operator+=(*iter->second);
    }
    WriteTxtCloud(ETC_PATH"DataToCloud/data/catch_cloud.txt", zx_points);

    for (auto & iter : clouds_xyz) {
        client.getCloudData(iter.first, iter.second);
        zx_points->operator+=(*iter.second);
    }
    WriteTxtCloud(ETC_PATH"DataToCloud/data/catch_cloud.txt", zx_points);
    return;

    while (true) {
        for (auto iter = clouds_xyz.begin(); iter != clouds_xyz.end(); iter++) {
            client.getCloudData(iter->first, iter->second);
        }

        int i = 0;
        for (const auto &iter: clouds_xyz) {
            viewer.removeAllPointClouds(view_port[i]);
            viewer.addPointCloud(iter.second, iter.first, view_port[i++]);
        }
        viewer.spinOnce();
    }
}

int CloudDataManager::sendClient() {
    DLOG(INFO) << "\n---Debug testRslidar begain\n";
//    XYZ2RYRBin("../etc/data/car.txt", "../etc/data/car.bin");
    RslidarManager lidar_manager;
    lidar_manager.init();
    // get lidar
    if (lidar_manager.getLidarNumber() == 0) {
        return false;
    }

    std::vector<int> id_list;
    lidar_manager.getIdList(id_list);
    RslidarDevice *lidar[id_list.size()];
    for (int i = 0; i < id_list.size(); i++) {
        lidar[i] = lidar_manager.getRslidar(id_list[i]);
    }

    RslidarMqttAsyncClient mqtt_client;
    mqtt_client.init();
    CloudPointPacket *packet = new LidarMessageRsHelios();
    CloudPointUnpacket unpacket;

    std::vector<unsigned char> data;

    std::map<std::string, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds_xyz;
    for(auto &l:lidar) {
        clouds_xyz.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>(LIDAR_SEND_TOPIC + std::to_string(l->id()), new pcl::PointCloud<pcl::PointXYZRGB>));
    }
#if VIEWER
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgb_pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    // 添加显示窗口
    pcl::PointCloud<pcl::PointALL>::Ptr zx_points(new pcl::PointCloud<pcl::PointALL>);
    pcl::visualization::PCLVisualizer viewer("viewer_0");
    viewer.setBackgroundColor(0, 0, 0);
    viewer.addCoordinateSystem(0.1);
    uint8_t color_list[6][3] = {{255, 0, 0},
                               {0, 255, 0},
                               {0, 0, 255},
                               {255, 255, 255},
                               {255, 255, 0},
                               {255, 0, 255}
    };
    sleep(1);
#endif

    auto t = std::chrono::steady_clock::now();
    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t;
    while (true) {

        static int ttt = 0;
        if (ttt > 100) {
            ttt = 0;
        }
        ttt++;
        for (int i = 0; i < lidar_manager.getLidarNumber(); i++) {
            t = std::chrono::steady_clock::now();
            lidar_manager.updateRSTransformParam(lidar[i]->id());
            data.clear();
//            cost = std::chrono::steady_clock::now() - t;
//            LOG(INFO) << "before get_last_cloud " << cost.count() * 1000 << " ms.";
            lidar[i]->get_last_cloud(data);
//            cost = std::chrono::steady_clock::now() - t;
//            LOG(INFO) << "after get_last_cloud " << cost.count() * 1000 << " ms.";
            // 测试代码，直接发的本地数据
//            GetRYRBin(ETC_PATH"DataToCloud/data/car.bin", data);
//            usleep(1000 * 25);
            if (!data.empty()) {
//                LOG(INFO) << "Get cloud data, data size: " << data.size();
//                cost = std::chrono::steady_clock::now() - t;
//                LOG(INFO) << "before packet " << cost.count() * 1000 << " ms.";
                if (packet->packet(data.data(), data.size())) {
//                    cost = std::chrono::steady_clock::now() - t;
//                    LOG(INFO) << "after packet " << cost.count() * 1000 << " ms.";
//                    DLOG(INFO) << "sned topic: " << LIDAR_SEND_TOPIC + std::to_string(lidar[i]->id());
                    mqtt_client.SendMessage(LIDAR_SEND_TOPIC + std::to_string(lidar[i]->id()), packet->data(),
                                            packet->size());
//                    cost = std::chrono::steady_clock::now() - t;
//                    LOG(INFO) << "SendMessage " << cost.count() * 1000 << " ms.";
#if VIEWER
                    zx_points->clear();
                    auto iter = clouds_xyz.find(LIDAR_SEND_TOPIC + std::to_string(lidar[i]->id()));
                    unpacket.unpacket(packet->data(), packet->size(), zx_points);
                    WriteTxtCloud(ETC_PATH"DataToCloud/data/" + std::to_string(lidar[i]->id() * 100 + ttt) + "_cloud.txt", zx_points);
//                    if(zx_points->size() > 0) {
//                        LOG(INFO) << zx_points->begin()->x << " " << zx_points->begin()->y;
//                    }
                    mqtt_client.setCloudData(iter->first, zx_points);
#endif
                }
                DLOG(INFO) << "Send cloud data over";
            }
        }
#if VIEWER
        viewer.removeAllPointClouds();
        int i = 0;
        for(auto &iter:clouds_xyz) {
//            LOG(INFO) << i;
//            if (iter.second->empty()) {
//                continue;
//            }
            mqtt_client.getCloudData(iter.first, iter.second, color_list[i++]);
            pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>rgb_color_handler(iter.second);
            viewer.addPointCloud<pcl::PointXYZRGB>(iter.second, rgb_color_handler, LIDAR_SEND_TOPIC + iter.first);
//            viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, LIDAR_SEND_TOPIC + iter.first);
//            viewer.addPointCloud(iter.second, LIDAR_SEND_TOPIC + iter.first);
        }
        viewer.spinOnce();
#endif
    }
    return 0;
}

int CloudDataManager::testRslidar() {
    RslidarManager lidar_manager;
    lidar_manager.init();
    // get lidar
    if (lidar_manager.getLidarNumber() == 0) {
        return false;
    }
    RslidarDevice *lidar[lidar_manager.getLidarNumber()];
    for (int i = 0; i < lidar_manager.getLidarNumber(); i++) {
        lidar[i] = lidar_manager.getRslidar(i);
    }

    std::vector<unsigned char> data;
    auto now = std::chrono::steady_clock::now();
    while (true) {
        for (int i = 0; i < lidar_manager.getLidarNumber(); i++) {
            std::chrono::duration<double> diff = std::chrono::steady_clock::now() - now;
            printf("---Debug time: %fms\n", diff.count() * 1000);
            lidar[i]->get_last_cloud(data);
            if (!data.empty()) {
                LOG(INFO) << "lidar data size" << data.size() << std::endl;
            }
        }
    }
}

int
CloudDataManager::test_call_back(MqttAsyncClient *client, char *topicName, int topicLen, MQTTAsync_message *message) {
    // 将unsigned char数组转换为时间戳（单位：毫秒）
    uint64_t timestamp = 0;
    std::memcpy(&timestamp, message->payload, message->payloadlen);

    // 还原为时间点
    auto time_point = std::chrono::time_point<std::chrono::steady_clock>(std::chrono::milliseconds(timestamp));

    std::chrono::duration<double> diffa = std::chrono::steady_clock::now() - time_point;
    printf("---Debug time: %fms\n", diffa.count() * 1000);

    return 1;
}

void CloudDataManager::testClient() {
    RslidarMqttAsyncClient mqtt_client;
    mqtt_client.init();
    auto subs = mqtt_client.getSubscribeEtc();

    // 循环等待消息
    while (true) {
        // 获取当前时间点
        auto now = std::chrono::steady_clock::now();

        // 将时间点转换为时间戳（单位：毫秒）
        auto timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();

        // 将时间戳转换为unsigned char数组
        unsigned char buffer[sizeof(timestamp)];
        for (size_t i = 0; i < sizeof(timestamp); i++) {
            buffer[i] = static_cast<unsigned char>((timestamp >> (8 * i)) & 0xFF);
        }

        mqtt_client.SendMessage(subs[0].topic() + "test-b", buffer, sizeof(buffer));
        usleep(1000 * 50);
    }
}

bool CloudDataManager::XYZ2RYRBin(const std::string &file, const std::string &file_bin) {
    std::vector<unsigned char> save_data;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    ReadTxtCloud(file, cloud);
    double x, y, z, x_y, r, roll, yaw;
    int i = 0;
    DLOG(INFO) << "get cloud number:" << cloud->size();
    for (auto &point: *cloud) {
        x = point.x * 100;
        y = point.y * 100;
        z = point.z * 100;
        r = sqrt(x * x + y * y + z * z);
//        DLOG(INFO) << point.x << " " << point.y << " " << point.z << " " << r;
        x_y = sqrt(x * x + y * y);
        roll = atan2(z, x_y);
        yaw = atan2(x, y);
        roll = (roll * 18000) / M_PI;
        yaw = (yaw * 18000) / M_PI;
        if (roll < 0) {
            roll += 36000;
        }
        if (yaw < 0) {
            yaw += 36000;
        }
        r = r / 0.25;
//        DLOG(INFO) << roll << " " << yaw << " " << r;
        save_data.push_back((unsigned short) r >> 8);
        save_data.push_back((unsigned short) r);
        save_data.push_back((unsigned short) roll >> 8);
        save_data.push_back((unsigned short) roll);
        save_data.push_back((unsigned short) yaw >> 8);
        save_data.push_back((unsigned short) yaw);
        save_data.push_back((unsigned short) 0);

//        ZX::PointALL pointa{};
//        double distance, roll_radian, yaw_radian;
//        distance = (double) ((save_data[i++] << 8 | save_data[i++]) * 0.25) * 0.01;
//        pointa.roll = (double) ((save_data[i++] << 8 | save_data[i++]) * 0.01);
//        pointa.yaw = (double) ((save_data[i++] << 8 | save_data[i++]) * 0.01);
//        pointa.pitch = 0;
//        roll_radian = pointa.roll * ZX::PI / 180.0;
//        yaw_radian = pointa.yaw * ZX::PI / 180.0;
//        pointa.x = distance * cos(roll_radian) * sin(yaw_radian);
//        pointa.y = distance * cos(roll_radian) * cos(yaw_radian);
//        pointa.z = distance * sin(roll_radian);
//        pointa.r3 = distance;
//        pointa.i = save_data[i++];
//        DLOG(INFO) << pointa.x << " " << pointa.y << " " << pointa.z << " " << distance;
//        DLOG(INFO) << pointa.roll << " " << pointa.yaw;
//        DLOG(INFO) << "===========================";
    }
    DLOG(INFO) << "save_data :" << save_data.size();
    std::fstream bin_file(file_bin, std::ofstream::trunc | ios::binary | std::ofstream::out);
    bin_file.write((const char *) save_data.data(), save_data.size());
    bin_file.close();
//    exit(EXIT_SUCCESS);
    return true;
}

bool CloudDataManager::GetRYRBin(const std::string &file_bin, std::vector<unsigned char> &data) {
    std::fstream bin_file(file_bin, ios::binary | std::ofstream::in);
    unsigned char uc;
    while (bin_file.read((char *) &uc, 1)) {
        data.emplace_back(uc);
    }
    bin_file.close();
    DLOG(INFO) << "data :" << data.size();
    usleep(1000 * 10);
    return true;
}