rfly_sensor.h

#ifndef RFLYSENSOR_H
#define RFLYSENSOR_H
 
#include <cstdint>
#include <vector>
#include <iostream>
#include "rfly_udp.h"
#ifdef _WIN32
#include <windows.h>
#else
#include <fcntl.h>
#include <sys/mman.h>
#include <unistd.h>
#endif
 
// 这是旧版本协议
// struct VisionSensorReq {
//     uint16_t checksum; //数据校验位，12345
//     uint16_t SeqID; //内存序号ID
//     uint16_t TypeID; //传感器类型ID
//     uint16_t TargetCopter; //绑定的目标飞机     //可改变
//     uint16_t TargetMountType; //绑定的类型    //可改变
//     uint16_t DataWidth;   //数据或图像宽度
//     uint16_t DataHeight; //数据或图像高度
//     uint16_t DataCheckFreq; //检查数据更新频率
//     uint16_t SendProtocol[8]; //传输类型（共享内存、UDP传输无压缩、UDP视频串流），IP地址，端口号，...
//     float CameraFOV;  //相机视场角（仅限视觉类传感器）  //可改变
//     float SensorPosXYZ[3]; // 传感器安装位置    //可改变
//     float SensorAngEular[3]; //传感器安装角度   //可改变
//     float otherParams[8]; //预留的8位数据位
// };
 
// 这是新版本协议
 
struct VisionSensorReq
{
    uint16_t checksum;        // 数据校验位，12345
    uint16_t SeqID;           // 内存序号ID
    uint16_t TypeID;          // 传感器类型ID
    uint16_t TargetCopter;    // 绑定的目标飞机     //可改变
    uint16_t TargetMountType; // 绑定的类型    //可改变
    uint16_t DataWidth;       // 数据或图像宽度
    uint16_t DataHeight;      // 数据或图像高度
    uint16_t DataCheckFreq;   // 检查数据更新频率
    uint16_t SendProtocol[8]; // 传输类型（共享内存、UDP传输无压缩、UDP视频串流），IP地址，端口号，...
 float CameraFOV;          // 相机视场角（仅限视觉类传感器）  //可改变
 float SensorPosXYZ[3];    // 传感器安装位置    //可改变
 float EulerOrQuat;        // 选择欧拉角或四元数方式，大于0.5就是四元数
 float SensorAngEuler[3];  // 传感器安装角度   //可改变
 float SensorAngQuat[4];   // 传感器安装四元数   //可改变
 float otherParams[16];    // 预留的16位数据位
 VisionSensorReq()
    {
        reset();
    }
 void reset()
    {
        checksum = 12345;
        SeqID = 0;
        TypeID = 0;
        TargetCopter = 0;
        TargetMountType = 0;
        DataWidth = 0;
        DataHeight = 0;
        DataCheckFreq = 0;
        CameraFOV = 0;
        EulerOrQuat = 0;
 for (int i = 0; i < 3; i++)
        {
            SensorPosXYZ[i] = 0;
            SensorAngEuler[i] = 0;
        }
 for (int i = 0; i < 16; i++)
        {
            otherParams[i] = 0;
        }
 for (int i = 0; i < 4; i++)
        {
            SensorAngQuat[i] = 0;
        }
    }
 void UpdateData(VisionSensorReq vsr)
    {
        checksum = vsr.checksum;
        TargetCopter = vsr.TargetCopter;
        TargetMountType = vsr.TargetMountType;
        DataWidth = vsr.DataWidth;
        DataHeight = vsr.DataHeight;
        DataCheckFreq = vsr.DataCheckFreq;
        CameraFOV = vsr.CameraFOV;
 for (int i = 0; i < 3; i++)
        {
            SensorPosXYZ[i] = vsr.SensorPosXYZ[i];
            SensorAngEuler[i] = vsr.SensorAngEuler[i];
        }
 for (int i = 0; i < 16; i++)
        {
            otherParams[i] = vsr.otherParams[i];
        }
 for (int i = 0; i < 4; i++)
        {
            SensorAngQuat[i] = vsr.SensorAngQuat[i];
        }
    }
};
 
 
class RflySensor
{
 
public:
 VisionSensorReq vis;
    std::string TargetIP;
 int windID;
 UDPSocket udp;
 int simTimeLastInt=0;
 bool hasImgRead=false;
 
 void sendVisionSensorReq()
    {
        this->udp.SendTo((const char *)&this->vis, sizeof(this->vis), this->TargetIP, 20010 + this->windID);
    }
 
 
 void updateImg(double t, uint8_T *data_total){
 
 bool Success = CreateShareMemory(vis.SeqID, vis.TypeID, vis.DataWidth, vis.DataHeight);
 
 if(!Success){
            std::cout << "CurTimeInt!=simTimeLastInt" << std::endl;
 int CurTimeInt = int(t);
 if (CurTimeInt!=simTimeLastInt){
                sendVisionSensorReq(); // send img req every 1 seconds
                simTimeLastInt=CurTimeInt;
            }
        }
 
 if(Success){
            GetImgBbrToRgb(data_total);
        }
 
    }
 
 // std::vector<VisionSensorReq>  VisionSensorReqVec; //存储所有传感器信息
public:
 RflySensor()
    {
        data = nullptr;
        dim = 1;
        dimSize = 1;
#ifdef _WIN32
        hMapFile = NULL;
#else
        shm_fd = -1;
#endif
    }
 ~RflySensor()
    {
        CloseShareMemory();
    }
 
 bool CreateShareMemory(int SeqID, int typeID, int imageWidth, int imageheight)
    {
        this->seqID = seqID;
        this->typeID = typeID;
 
        this->DataWidth = imageWidth;
        this->DataHeight = imageheight;
 
#ifdef _WIN32
 wchar_t full_shm_name[256];
        swprintf(full_shm_name, 256, L"RflySim3DImg_%d", SeqID);
 
 if (hMapFile == NULL)
        {
 // 打开内存共享
            hMapFile = OpenFileMappingW(FILE_MAP_ALL_ACCESS, FALSE, full_shm_name);
 if (hMapFile == NULL)
            {
                std::cerr << "Failed to open UE shared memory" << std::endl;
 return false;
            }
        }
#else
 char full_shm_name[256];
        snprintf(full_shm_name, 256, "RflySim3DImg_%d", SeqID);
 if (shm_fd == -1)
        {
            shm_fd = shm_open(full_shm_name, O_RDONLY, 0666);
 if (shm_fd == -1)
            {
                std::cerr << "Failed to open UE shared memory" << std::endl;
 return false;
            }
        }
 
#endif
 
 return true;
    }
 
 unsigned char *ReadImageData()
    {
#ifdef _WIN32
        LPVOID lpMapAddress = MapViewOfFile(hMapFile, FILE_MAP_ALL_ACCESS, 0, 0, 0);
 if (lpMapAddress == NULL)
        {
            std::cerr << "Failed to map image shared memory" << std::endl;
 return nullptr;
        }
        BYTE *mm = static_cast<BYTE *>(lpMapAddress);
 
 // 获取文件映射对象的大小
        DWORD size = GetFileSize(hMapFile, NULL);
 
#else
 unsigned char *mm = static_cast<unsigned char *>(mmap(NULL, DataWidth * DataHeight * dim * dimSize, PROT_READ, MAP_SHARED, shm_fd, 0));
 if (mm == MAP_FAILED)
        {
            std::cerr << "Failed to map image shared memory" << std::endl;
 return nullptr;
        }
#endif
 
 unsigned char *pImageData = static_cast<unsigned char *>(mm);
 unsigned char busyFlag = pImageData[0];
 
 if (busyFlag == 2)
        {
            pImageData[0] = 3;
 // 根据不同的图片类型计算图像数据的大小
 if (typeID == 1)
            {
                dim = 3;
                dimSize = 1;
            }
 else if (typeID == 2 || typeID == 3)
            {
                dim = 1;
                dimSize = 1;
            }
 
            dataSize = DataWidth * DataHeight * dim * dimSize;
 
 if (dataSize != lastDataSize && data != nullptr)
            {
 delete[] data;
            }
 
 if (data == nullptr)
            {
                data = new unsigned char[dataSize];
 // lastDataSize = dataSize;
            }
 
 
 
 int offset = 1 + 8; // 根据需要调整偏移量的计算方式
 for (size_t i = 0; i < dataSize; ++i)
            {
                data[i] = pImageData[i + offset];
            }
            pImageData[0] = 4;
        }
 
#ifdef _WIN32
        UnmapViewOfFile(lpMapAddress);
#else
        munmap(mm, DataWidth * DataHeight * dim * dimSize);
#endif
 
 return data;
    }
 
 void CloseShareMemory()
    {
#ifdef _WIN32
 if (data != NULL)
        {
            UnmapViewOfFile(data);
 delete data;
            data = nullptr;
        }
 if (hMapFile != NULL)
        {
            CloseHandle(hMapFile);
            hMapFile = NULL;
        }
#else
 if (data != NULL)
        {
            munmap(data, dataSize);
 delete data;
            data = nullptr;
        }
 if (shm_fd != -1)
        {
            close(shm_fd);
            shm_fd = -1;
        }
#endif
 
 if (imageData != NULL)
        {
 delete imageData;
            imageData = nullptr;
        }
    }
 
 bool GetImgBbrToRgb(uint8_t *data_total)
    {
 unsigned char *data = ReadImageData();
 if (data == nullptr)
        {
            std::cerr << "Failed to read image data" << std::endl;
 return false;
        }
 // static unsigned char* imageData = nullptr;
 
 if (dataSize != lastDataSize && imageData != nullptr)
        {
 delete[] imageData;
        }
 
 if (imageData == NULL){
            imageData = new unsigned char[dataSize];
            lastDataSize = dataSize;
        }
 
 
 int Width = this->DataWidth;
 int Height = this->DataHeight;
 int Dim = this->dim;
 int Imgwh = Width * Height;
 const int newWidth = Height;
 const int newHeight = Width;
 const int newDepth = Dim;
 
 if (Dim == 1)
            memcpy(data_total, data, dataSize);
 else if (Dim == 3)
        {
 for (int i = 0; i < Dim; ++i)
            {
 //            for (int j = 0; j < Imgwh; ++j) {
 //                data_total[i * Imgwh + j] = data[(Dim * j) + (Dim - i - 1)];
 //            }
 
 for (int h = 0; h < Height; ++h)
                {
 for (int w = 0; w < Width; ++w)
                    {
                        imageData[i * Width * Height + (h * Width + w)] = data[(Dim * (h * Width + w)) + (Dim - i - 1)];
                    }
                }
            }
 // for (int i = 0; i < Dim; ++i) {
 for (int i = 0; i < Dim; ++i)
            {
 for (int w = Width - 1; w >= 0; --w)
                {
 for (int h = Height - 1; h >= 0; --h)
                    {
 // data_total[i * Width * Height + ((Height-1-h) * Width + (Width-1-w))] = imageData[(Dim * (h * Width + w)) + (Dim - i - 1)];
                        data_total[i * Width * Height + (Width - 1 - w) * Height + (Height - 1 - h)] = imageData[(i * Width * Height) + ((Height - 1 - h) * Width + w)];
                    }
                }
            }
        }
 return true;
    }
 
private:
 unsigned char *data;
 long dataSize,lastDataSize;
 int DataWidth = 640;
 int DataHeight = 480;
 int typeID;
 int seqID;
 int dim;
 int dimSize;
 unsigned char *imageData = nullptr;
 //    int* pData;
 
#ifdef _WIN32
    HANDLE hMapFile;
    HANDLE UEhMapFile;
#else
 int shm_fd;
 int ue_shm_fd;
#endif
};
 
#endif // RFLYSENSOR_H