456 lines
16 KiB
C++
456 lines
16 KiB
C++
#include <iostream>
|
||
#include <map>
|
||
#include <string>
|
||
|
||
#include "epiceye.h"
|
||
#include "httplib.h"
|
||
#include "nlohmann_json.hpp"
|
||
|
||
// 连接超时时间：300 milliseconds
|
||
#define HTTP_CONNECTION_TIMEOUT 300000
|
||
// Read 超时时间 30秒
|
||
#define HTTP_READ_TIMEOUT 30
|
||
// write 超时时间 30秒
|
||
#define HTTP_WRITE_TIMEOUT 30
|
||
|
||
httplib::Result httpGet(std::string ip, std::string uri) {
|
||
httplib::Client client(ip);
|
||
client.set_connection_timeout(0, HTTP_CONNECTION_TIMEOUT);
|
||
client.set_read_timeout(HTTP_READ_TIMEOUT, 0);
|
||
client.set_write_timeout(HTTP_WRITE_TIMEOUT, 0);
|
||
return client.Get(uri.c_str());
|
||
}
|
||
|
||
httplib::Result httpPost(std::string ip, std::string uri) {
|
||
httplib::Client client(ip);
|
||
client.set_connection_timeout(0, HTTP_CONNECTION_TIMEOUT);
|
||
client.set_read_timeout(HTTP_READ_TIMEOUT, 0);
|
||
client.set_write_timeout(HTTP_WRITE_TIMEOUT, 0);
|
||
return client.Post(uri.c_str());
|
||
}
|
||
|
||
httplib::Result httpPut(std::string ip, std::string uri, std::string content) {
|
||
httplib::Client client(ip);
|
||
client.set_connection_timeout(0, HTTP_CONNECTION_TIMEOUT);
|
||
client.set_read_timeout(HTTP_READ_TIMEOUT, 0);
|
||
client.set_write_timeout(HTTP_WRITE_TIMEOUT, 0);
|
||
return client.Put(uri.c_str(), content, "application/json");
|
||
}
|
||
|
||
#ifdef _WIN32
|
||
#include <Winsock2.h>
|
||
#include <stdio.h>
|
||
#endif
|
||
|
||
#define EPICEYE_SDK_VERSION "3.3.0"
|
||
#define EPICRAW2_HEADER_SIZE 40
|
||
|
||
#define CAMERA_DISCOVERY_PORT 5665
|
||
|
||
#define JSON_KEY_SN "sn"
|
||
#define JSON_KEY_IP "ip"
|
||
#define JSON_KEY_ALIAS "alias"
|
||
#define JSON_KEY_MODEL "model"
|
||
#define JSON_KEY_WIDTH "width"
|
||
#define JSON_KEY_HEIGHT "height"
|
||
|
||
#define JSON_KEY_CAMERA_MATRIX "cameraMatrix"
|
||
#define JSON_KEY_DISTORTION "distortion"
|
||
#define JSON_KEY_M "m"
|
||
#define JSON_KEY_K1 "k1"
|
||
#define JSON_KEY_K2 "k2"
|
||
#define JSON_KEY_K3 "k3"
|
||
#define JSON_KEY_P1 "p1"
|
||
#define JSON_KEY_P2 "p2"
|
||
|
||
namespace TFTech {
|
||
|
||
static std::map<std::string, std::vector<float>> undistortLutMap;
|
||
const std::vector<uint32_t> cameraDiscoveryPorts = {5665, 5666, 5667, 5668};
|
||
|
||
bool isValid(httplib::Result &res, std::string message) {
|
||
if (!res) {
|
||
std::cerr << message << ": " << "request failed!" << std::endl;
|
||
return false;
|
||
}
|
||
if (res->status != 200) {
|
||
std::cerr << message << ": " << res->status << " " << httplib::detail::status_message(res->status) << std::endl;
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
std::string EpicEye::getSDKVersion() {
|
||
return EPICEYE_SDK_VERSION;
|
||
}
|
||
|
||
bool requestUndistortLut(std::string ip) {
|
||
EpicEyeInfo info;
|
||
if (!EpicEye::getInfo(ip, info)) {
|
||
return false;
|
||
}
|
||
httplib::Result res = httpGet(ip, "/api/UndistortLut");
|
||
|
||
if (!res) {
|
||
std::cerr << "requestUndistortLut" << ": "
|
||
<< "request failed!" << std::endl;
|
||
return false;
|
||
}
|
||
if (res->status != 200) {
|
||
if(res->status == 404) {
|
||
std::vector<float> emptyArray;
|
||
undistortLutMap[ip] = emptyArray;
|
||
return true;
|
||
} else {
|
||
std::cerr << "requestUndistortLut" << ": " << res->status << " " << httplib::detail::status_message(res->status) << std::endl;
|
||
return false;
|
||
}
|
||
}
|
||
|
||
const float *buffer = reinterpret_cast<const float *>(res->body.c_str());
|
||
uint32_t bufferSize = info.height * info.width * 2;
|
||
std::vector<float> undistortLut(buffer, buffer + bufferSize);
|
||
undistortLutMap[ip] = undistortLut;
|
||
return true;
|
||
}
|
||
|
||
//获取相机信息
|
||
bool EpicEye::getInfo(std::string ip, EpicEyeInfo &info) {
|
||
httplib::Result res = httpGet(ip, "/api/EpicEye/Info");
|
||
if (!isValid(res, "getInfo")) {
|
||
return false;
|
||
}
|
||
nlohmann::json infoJson = nlohmann::json::parse(res->body);
|
||
if (infoJson.contains(JSON_KEY_SN) &&
|
||
infoJson.contains(JSON_KEY_IP) &&
|
||
infoJson.contains(JSON_KEY_MODEL) &&
|
||
infoJson.contains(JSON_KEY_ALIAS) &&
|
||
infoJson.contains(JSON_KEY_WIDTH) &&
|
||
infoJson.contains(JSON_KEY_HEIGHT)) {
|
||
// 直接赋值，避免聚合初始化问题
|
||
info.SN = infoJson[JSON_KEY_SN].get<std::string>();
|
||
info.IP = infoJson[JSON_KEY_IP].get<std::string>();
|
||
info.model = infoJson[JSON_KEY_MODEL].get<std::string>();
|
||
info.alias = infoJson[JSON_KEY_ALIAS].get<std::string>();
|
||
info.width = infoJson[JSON_KEY_WIDTH].get<uint32_t>();
|
||
info.height = infoJson[JSON_KEY_HEIGHT].get<uint32_t>();
|
||
return true;
|
||
} else {
|
||
std::cerr << "json parse error!" << std::endl;
|
||
return false;
|
||
}
|
||
}
|
||
|
||
//触发拍照
|
||
bool EpicEye::triggerFrame(std::string ip, std::string &frameID, bool pointCloud) {
|
||
std::string paramPointCloud = pointCloud ? "true" : "false";
|
||
std::string uri = "/api/Frame?pointCloud=" + paramPointCloud;
|
||
httplib::Result res = httpPost(ip, uri);
|
||
if (!isValid(res, "triggerFrame")) {
|
||
return false;
|
||
}
|
||
frameID = res->body;
|
||
return true;
|
||
}
|
||
|
||
bool EpicEye::getImage(std::string ip, std::string frameID, void *imageBuffer, int &pixelByteSize) {
|
||
std::string uri = "/api/Frame?frameId=" + frameID;
|
||
httplib::Result res = httpGet(ip, uri);
|
||
if (!isValid(res, "getImage")) {
|
||
return false;
|
||
}
|
||
const char *bytes = res->body.c_str();
|
||
int width = *(const int *)(bytes + 8);
|
||
int height = *(const int *)(bytes + 12);
|
||
int dataType = *(const int *)(bytes + 16);
|
||
int cameraMatrixLength = *(const int *)(bytes + 20);
|
||
int distortionLength = *(const int *)(bytes + 24);
|
||
int configStrLength = *(const int *)(bytes + 28);
|
||
int depthDataLength = *(const int *)(bytes + 32);
|
||
int imageDataLength = *(const int *)(bytes + 36);
|
||
memcpy(
|
||
imageBuffer,
|
||
bytes + EPICRAW2_HEADER_SIZE + cameraMatrixLength + distortionLength + configStrLength + depthDataLength,
|
||
imageDataLength
|
||
);
|
||
pixelByteSize = imageDataLength / width / height;
|
||
return true;
|
||
}
|
||
|
||
bool EpicEye::getPointCloud(std::string ip, std::string frameID, float *pointCloudBuffer) {
|
||
std::string uri = "/api/Frame?frameId=" + frameID;
|
||
httplib::Result res = httpGet(ip, uri);
|
||
if (!isValid(res, "getPointCloud")) {
|
||
return false;
|
||
}
|
||
if(undistortLutMap.find(ip) == undistortLutMap.end()) {
|
||
if(!requestUndistortLut(ip)) {
|
||
std::cout << "not using undistortLutMap" << std::endl;
|
||
}
|
||
}
|
||
const std::vector<float> &undistortLut = undistortLutMap[ip];
|
||
bool useUndistortLut = undistortLut.size() > 0 ? true: false;
|
||
|
||
const char *bytes = res->body.c_str();
|
||
int width = *(const int *)(bytes + 8);
|
||
int height = *(const int *)(bytes + 12);
|
||
int dataType = *(const int *)(bytes + 16);
|
||
int cameraMatrixLength = *(const int *)(bytes + 20);
|
||
int distortionLength = *(const int *)(bytes + 24);
|
||
int configStrLength = *(const int *)(bytes + 28);
|
||
int depthDataLength = *(const int *)(bytes + 32);
|
||
int imageDataLength = *(const int *)(bytes + 36);
|
||
|
||
double A = *(const double *)(bytes + EPICRAW2_HEADER_SIZE);
|
||
double B = *(const double *)(bytes + EPICRAW2_HEADER_SIZE + 16);
|
||
double C = *(const double *)(bytes + EPICRAW2_HEADER_SIZE + 32);
|
||
double D = *(const double *)(bytes + EPICRAW2_HEADER_SIZE + 40);
|
||
const char *data = bytes + EPICRAW2_HEADER_SIZE + cameraMatrixLength + distortionLength + configStrLength;
|
||
for (int i = 0; i < height; i++) {
|
||
for (int j = 0; j < width; j++) {
|
||
float Z = *(const float *)(data + (i * width + j) * 4);
|
||
if (Z > 0.1 && Z < 6000) {
|
||
uint32_t index = i * width * 2 + j *2;
|
||
float u = (float)j;
|
||
float v = (float)i;
|
||
if(useUndistortLut) {
|
||
u = undistortLut[index];
|
||
v = undistortLut[index + 1];
|
||
}
|
||
*(pointCloudBuffer + (i * width + j) * 3) = (float)(Z * (u - B) / A);
|
||
*(pointCloudBuffer + (i * width + j) * 3 + 1) = (float)(Z * (v - D) / C);
|
||
*(pointCloudBuffer + (i * width + j) * 3 + 2) = Z;
|
||
} else {
|
||
*(pointCloudBuffer + (i * width + j) * 3) = NAN;
|
||
*(pointCloudBuffer + (i * width + j) * 3 + 1) = NAN;
|
||
*(pointCloudBuffer + (i * width + j) * 3 + 2) = NAN;
|
||
}
|
||
}
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool EpicEye::getDepth(std::string ip, std::string frameID, float *depth) {
|
||
std::string uri = "/api/Depth?frameId=" + frameID;
|
||
httplib::Result res = httpGet(ip, uri);
|
||
if (!isValid(res, "getDepth")) {
|
||
return false;
|
||
}
|
||
const char *bytes = res->body.c_str();
|
||
int width = *(const int *)(bytes + 8);
|
||
int height = *(const int *)(bytes + 12);
|
||
int dataType = *(const int *)(bytes + 16);
|
||
int cameraMatrixLength = *(const int *)(bytes + 20);
|
||
int distortionLength = *(const int *)(bytes + 24);
|
||
int configStrLength = *(const int *)(bytes + 28);
|
||
int depthDataLength = *(const int *)(bytes + 32);
|
||
int imageDataLength = *(const int *)(bytes + 36);
|
||
memcpy(
|
||
depth,
|
||
bytes + EPICRAW2_HEADER_SIZE + cameraMatrixLength + distortionLength + configStrLength,
|
||
depthDataLength
|
||
);
|
||
return true;
|
||
}
|
||
|
||
bool EpicEye::getConfig(std::string ip, nlohmann::json &configJson) {
|
||
std::string uri = "/api/CameraConfig";
|
||
httplib::Result res = httpGet(ip, uri);
|
||
if (!isValid(res, "getConfig")) {
|
||
return false;
|
||
}
|
||
try {
|
||
configJson = nlohmann::json::parse(res->body);
|
||
return true;
|
||
} catch(...) {
|
||
std::cerr << "json parse error!" << std::endl;
|
||
return false;
|
||
}
|
||
}
|
||
|
||
bool EpicEye::setConfig(std::string ip, const nlohmann::json &configJson) {
|
||
std::string uri = "/api/CameraConfig";
|
||
std::string content = configJson.dump();
|
||
httplib::Result res = httpPut(ip, uri, content);
|
||
if (!isValid(res, "setConfig")) {
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool EpicEye::getCameraMatrix(std::string ip, std::vector<float> &cameraMatrix) {
|
||
std::string uri = "/api/CameraParameters/Intrinsic/CameraMatrix";
|
||
httplib::Result res = httpGet(ip, uri);
|
||
if (!isValid(res, "getIntrinsic")) {
|
||
return false;
|
||
}
|
||
nlohmann::json cameraMatrixJson = nlohmann::json::parse(res->body);
|
||
cameraMatrix.clear();
|
||
for (const auto &element : cameraMatrixJson) {
|
||
cameraMatrix.push_back(element);
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool EpicEye::getDistortion(std::string ip, std::vector<float> &distortion) {
|
||
std::string uri = "/api/CameraParameters/Intrinsic/Distortion";
|
||
httplib::Result res = httpGet(ip, uri);
|
||
if (!isValid(res, "getDistortion")) {
|
||
return false;
|
||
}
|
||
nlohmann::json distortionJson = nlohmann::json::parse(res->body);
|
||
distortion.clear();
|
||
for (const auto &element : distortionJson) {
|
||
distortion.push_back(element);
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool EpicEye::searchCamera(std::vector<EpicEyeInfo> &cameraList) {
|
||
cameraList.clear();
|
||
|
||
int socketUDPv4 = socket(AF_INET, SOCK_DGRAM, 0);
|
||
int socketUDPv6 = socket(AF_INET6, SOCK_DGRAM, 0);
|
||
if (socketUDPv4 < 0 || socketUDPv6 < 0) {
|
||
return false;
|
||
}
|
||
|
||
struct sockaddr_in addr_serv_v4;
|
||
memset(&addr_serv_v4, 0, sizeof(struct sockaddr_in));
|
||
addr_serv_v4.sin_family = AF_INET;
|
||
addr_serv_v4.sin_port = htons(CAMERA_DISCOVERY_PORT);
|
||
addr_serv_v4.sin_addr.s_addr = htonl(INADDR_ANY);
|
||
|
||
struct sockaddr_in6 addr_serv_v6;
|
||
memset(&addr_serv_v6, 0, sizeof(struct sockaddr_in6));
|
||
addr_serv_v6.sin6_family = AF_INET6;
|
||
addr_serv_v6.sin6_port = htons(CAMERA_DISCOVERY_PORT);
|
||
addr_serv_v6.sin6_addr = in6addr_any;
|
||
|
||
int len_v4, len_v6;
|
||
len_v4 = sizeof(addr_serv_v4);
|
||
len_v6 = sizeof(addr_serv_v6);
|
||
|
||
#ifdef _WIN32
|
||
int nNetTimeout = 4000;
|
||
if (setsockopt(socketUDPv4, SOL_SOCKET, SO_RCVTIMEO, (char*)&nNetTimeout, sizeof(int)) < 0 ||
|
||
setsockopt(socketUDPv6, SOL_SOCKET, SO_RCVTIMEO, (char*)&nNetTimeout, sizeof(int)) < 0) {
|
||
std::cerr << "searchCamera:: can not set timeout for udp socket!" << std::endl;
|
||
return false;
|
||
}
|
||
#else
|
||
struct timeval tv;
|
||
tv.tv_sec = 4;
|
||
tv.tv_usec = 0;
|
||
if (setsockopt(socketUDPv4, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0 ||
|
||
setsockopt(socketUDPv6, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) {
|
||
std::cerr << "searchCamera:: can not set timeout for udp socket!" << std::endl;
|
||
return false;
|
||
}
|
||
#endif
|
||
bool bindv4Success = false, bindv6Success = false;
|
||
for (const auto& port : cameraDiscoveryPorts) {
|
||
if (!bindv4Success) {
|
||
addr_serv_v4.sin_port = htons(port);
|
||
if (bind(socketUDPv4, (struct sockaddr *)&addr_serv_v4, sizeof(addr_serv_v4)) < 0) {
|
||
std::cerr << "searchCamera:: ipv4 bind port " << port << " failed!" << std::endl;
|
||
} else {
|
||
bindv4Success = true;
|
||
}
|
||
}
|
||
if (!bindv6Success) {
|
||
addr_serv_v6.sin6_port = htons(port);
|
||
if (bind(socketUDPv6, (struct sockaddr *)&addr_serv_v6, sizeof(addr_serv_v6)) < 0) {
|
||
std::cerr << "searchCamera:: ipv6 bind port " << port << " failed!" << std::endl;
|
||
} else {
|
||
bindv6Success = true;
|
||
}
|
||
}
|
||
if (bindv4Success && bindv6Success) {
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (!(bindv4Success && bindv6Success)) {
|
||
return false;
|
||
}
|
||
|
||
ip_mreq mreq_v4;
|
||
mreq_v4.imr_multiaddr.s_addr = inet_addr("224.0.0.251");
|
||
mreq_v4.imr_interface.s_addr = htonl(INADDR_ANY);
|
||
if (setsockopt(socketUDPv4, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *)&mreq_v4, sizeof(mreq_v4)) < 0) {
|
||
std::cerr << "searchCamera:: failed to join multicast group for IPv4!" << std::endl;
|
||
return false;
|
||
}
|
||
|
||
ipv6_mreq mreq_v6;
|
||
inet_pton(AF_INET6, "ff02::fb", &mreq_v6.ipv6mr_multiaddr);
|
||
mreq_v6.ipv6mr_interface = 0; // Use the default interface
|
||
if (setsockopt(socketUDPv6, IPPROTO_IPV6, IPV6_JOIN_GROUP, (char *)&mreq_v6, sizeof(mreq_v6)) < 0) {
|
||
std::cerr << "searchCamera:: failed to join multicast group for IPv6!" << std::endl;
|
||
return false;
|
||
}
|
||
|
||
int recv_num_v4, recv_num_v6;
|
||
char recvBuf_v4[1025], recvBuf_v6[1025];
|
||
std::vector<std::string> ipList;
|
||
struct sockaddr_in addr_client_v4;
|
||
struct sockaddr_in6 addr_client_v6;
|
||
|
||
auto pushBackIfNotExists = [&](const std::string& infoStr) {
|
||
nlohmann::json infoJson = nlohmann::json::parse(infoStr);
|
||
if (infoJson.contains(JSON_KEY_SN) &&
|
||
infoJson.contains(JSON_KEY_IP)) {
|
||
EpicEyeInfo info;
|
||
info.SN = infoJson[JSON_KEY_SN];
|
||
info.IP = infoJson[JSON_KEY_IP];
|
||
bool exists = std::any_of(cameraList.begin(), cameraList.end(), [&](const EpicEyeInfo& existingInfo) {
|
||
return existingInfo.IP == info.IP;
|
||
});
|
||
if (!exists) {
|
||
cameraList.push_back(info);
|
||
}
|
||
}
|
||
};
|
||
|
||
for (int i = 0; i < 6; i++) {
|
||
recv_num_v4 = recvfrom(socketUDPv4, recvBuf_v4, sizeof(recvBuf_v4) - 1, 0,
|
||
(struct sockaddr *)&addr_client_v4, (socklen_t *)&len_v4);
|
||
|
||
if (recv_num_v4 > 0) {
|
||
char addressArray[INET_ADDRSTRLEN] = {'\0'};
|
||
inet_ntop(AF_INET, &addr_client_v4.sin_addr, addressArray, sizeof(addressArray));
|
||
std::string peerIP = addressArray;
|
||
recvBuf_v4[recv_num_v4] = '\0';
|
||
std::string infoStr = (std::string)recvBuf_v4;
|
||
pushBackIfNotExists(infoStr);
|
||
}
|
||
|
||
recv_num_v6 = recvfrom(socketUDPv6, recvBuf_v6, sizeof(recvBuf_v6) - 1, 0,
|
||
(struct sockaddr *)&addr_client_v6, (socklen_t *)&len_v6);
|
||
|
||
if (recv_num_v6 > 0) {
|
||
char addressArray[INET6_ADDRSTRLEN] = {'\0'};
|
||
inet_ntop(AF_INET6, &addr_client_v6.sin6_addr, addressArray, sizeof(addressArray));
|
||
std::string peerIP = addressArray;
|
||
recvBuf_v6[recv_num_v6] = '\0';
|
||
std::string infoStr = (std::string)recvBuf_v6;
|
||
pushBackIfNotExists(infoStr);
|
||
}
|
||
}
|
||
|
||
#ifdef _WIN32
|
||
closesocket(socketUDPv4);
|
||
closesocket(socketUDPv6);
|
||
#else
|
||
close(socketUDPv4);
|
||
close(socketUDPv6);
|
||
#endif
|
||
|
||
if (cameraList.size() == 0) {
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
} // namespace TFTech
|