|
|
// Pavo2 Lidar SDK - 雷达数据采集程序
|
|
|
// 功能1：获取单帧雷达数据 + 输出鸟瞰图(ppm)
|
|
|
// 功能2：获取多秒雷达数据，输出 x,y,时间戳(ms) 文件 + 深度图(ppm)
|
|
|
|
|
|
#include <iostream>
|
|
|
#include <fstream>
|
|
|
#include <vector>
|
|
|
#include <chrono>
|
|
|
#include <iomanip>
|
|
|
#include <thread>
|
|
|
#include <mutex>
|
|
|
#include <cmath>
|
|
|
#include <algorithm>
|
|
|
#include <cstring>
|
|
|
#include <cstdlib>
|
|
|
#include <ctime>
|
|
|
#include <direct.h> // _mkdir
|
|
|
#include <sstream>
|
|
|
#include <unordered_map>
|
|
|
|
|
|
// pavo2_driver.h 内部会引入 winsock2.h，必须在 windows.h 之前
|
|
|
// NOMINMAX: MinGW GCC 在 os_defines.h 中已自动定义，无需手动定义
|
|
|
//#include "pavo2_driver.h"
|
|
|
#include "PAVO2_SDK/include/pavo2_driver.h"
|
|
|
#ifdef _WIN32
|
|
|
#include <windows.h>
|
|
|
#endif
|
|
|
|
|
|
// stb single-header JPEG 编码器
|
|
|
#define STB_IMAGE_WRITE_IMPLEMENTATION
|
|
|
#include "PAVO2_SDK/include/stb_image_write.h"
|
|
|
|
|
|
// ==================== 配置参数 ====================
|
|
|
//const std::string LIDAR_IP = "192.168.0.170"; // 雷达IP地址（必填）
|
|
|
//const uint16_t Leida_PORT = 2368; // 雷达数据端口
|
|
|
const uint32_t OPEN_TIMEOUT_MS = 3000; // 连接超时(ms)
|
|
|
const uint16_t FRAME_TIMEOUT_MS = 2000; // 获取帧数据超时(ms)
|
|
|
//driver.pavo2_open(Leida_IP, Leida_PORT, OPEN_TIMEOUT_MS)//0713改
|
|
|
// -------------------配置参数 0713增加------------------//
|
|
|
static std::string Leida_IP = "192.168.0.170";
|
|
|
static uint16_t Leida_PORT = 2368;
|
|
|
static int Collect_Duration_S = 6;
|
|
|
static double Speed_Z = 20.0;
|
|
|
static bool Log_Flag = true;
|
|
|
static double Z_RANGE_MM = 50.0; // Z截面区间长度(mm)
|
|
|
static double Y_RANGE_MM = 80.0; // Y截面区间长度(mm): 只取[Y最小, Y最小+此值]的点参与
|
|
|
static double Lidar_Hz = 50.0; // 雷达帧率(Hz)，用于计算矩形长
|
|
|
static int Rect_Width_PX = 8; // 截面矩形宽度(px)
|
|
|
static double Rect_Length_Ratio = 1.5; // 矩形长倍率: 矩形长=速度/帧率×倍率
|
|
|
static std::string Run_Mode = "exe"; // exe=直接检测一次, tcp=服务端模式
|
|
|
static uint16_t Tcp_Port = 9601; // TCP模式监听端口
|
|
|
static double Floor_Height = 1600.0; // 地板高度(mm): 物品高度 = Floor_Height - Y轴(count计数用)
|
|
|
static double Count_Base_Tolerance = 50.0; // 分层初始冗余 50mm(count分层用)
|
|
|
static double Count_Additional_Tolerance_Per_Level = 20.0; // 每层增加 20mm 冗余(count分层用)
|
|
|
static int Point_Dilate = 8; // count画layer图: 每个点膨胀 Point_Dilate×Point_Dilate 像素
|
|
|
static std::string Z_Interpolate_Mode = "none"; // count z轴空隙插值模式: none=不插值 / CopyTranslation=同圈复制平移 / AngleBucketInterpolation=角度桶插值
|
|
|
|
|
|
static int Tuo_Wei = 2; // 拖尾滤波0=关, 1=弱, 2=中, 3=强
|
|
|
static int Qian_Chong = 2; //前冲滤波0=关, 1=弱, 2=中, 3=强
|
|
|
static int Yuan_Duan = 1; // 远端高反滤波0=关, 1=弱, 2=强
|
|
|
static bool Jin_Duan = true; // 近端高反识别true开，false关
|
|
|
//----------------------------------------------------//
|
|
|
// 多边形过滤开关（false 则禁用，全部保留）
|
|
|
const bool FILTER_ENABLE = true;
|
|
|
|
|
|
// 多边形过滤区域（顶点坐标，单位: mm）
|
|
|
|
|
|
static std::vector<std::pair<double, double>> Filter_Polygon = {
|
|
|
{-650, 0}, {-650, 670}, {650, 670}, {650, 0}
|
|
|
};
|
|
|
static int Filter_Poly_N = (int)Filter_Polygon.size(); // 自动获取顶点数 // <20>Զ<EFBFBD><D4B6><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
// 当 true 时，所有Y值上移使最小Y≥0（消除负数）
|
|
|
const bool SHIFT_Y_POSITIVE = true;
|
|
|
|
|
|
// Z轴：速度(cm/s)，0=使用时间(ms)，>0=Z轴=速度×时间=移动距离(mm)
|
|
|
//const double Speed_Z = 20.0;
|
|
|
|
|
|
// 过滤映射到原点(0,0)的无效点（雷达噪点）
|
|
|
// x和y同时接近0时视为原点无效点，丢弃该点
|
|
|
//---------------加载配置文件---------------------------//
|
|
|
// 去除字符串首尾空白
|
|
|
static std::string trim_str(const std::string& s)
|
|
|
{
|
|
|
size_t b = s.find_first_not_of(" \t\r\n");
|
|
|
if (b == std::string::npos) return "";
|
|
|
size_t e = s.find_last_not_of(" \t\r\n");
|
|
|
return s.substr(b, e - b + 1);
|
|
|
}
|
|
|
|
|
|
// GBK(ANSI) → UTF-8：日志文件按UTF-8写入，避免编辑器打开乱码
|
|
|
// （exe内字符串按 -fexec-charset=GBK 编译，控制台GBK显示正常，写文件需转UTF-8）
|
|
|
static std::string gbk_to_utf8(const std::string& s)
|
|
|
{
|
|
|
#ifdef _WIN32
|
|
|
if (s.empty()) return s;
|
|
|
int wlen = MultiByteToWideChar(CP_ACP, 0, s.c_str(), (int)s.size(), nullptr, 0);
|
|
|
if (wlen <= 0) return s;
|
|
|
std::wstring wbuf(wlen, L'\0');
|
|
|
MultiByteToWideChar(CP_ACP, 0, s.c_str(), (int)s.size(), &wbuf[0], wlen);
|
|
|
int ulen = WideCharToMultiByte(CP_UTF8, 0, wbuf.c_str(), wlen, nullptr, 0, nullptr, nullptr);
|
|
|
if (ulen <= 0) return s;
|
|
|
std::string ubuf(ulen, '\0');
|
|
|
WideCharToMultiByte(CP_UTF8, 0, wbuf.c_str(), wlen, &ubuf[0], ulen, nullptr, nullptr);
|
|
|
return ubuf;
|
|
|
#else
|
|
|
return s;
|
|
|
#endif
|
|
|
}
|
|
|
|
|
|
void load_config(const std::string& path = "./config/config")
|
|
|
{
|
|
|
std::ifstream fin(path);
|
|
|
if (!fin.is_open()) {
|
|
|
std::cerr << "[WARN] 未找到 config配置文件" << std::endl;
|
|
|
exit(-1);
|
|
|
}
|
|
|
|
|
|
std::string line;
|
|
|
while (std::getline(fin, line)) {
|
|
|
if (line.find("Leida_Ip:") == 0) {
|
|
|
Leida_IP = line.substr(line.find(':') + 1);
|
|
|
}
|
|
|
else if (line.find("Leida_Port:") == 0) {
|
|
|
Leida_PORT = static_cast<uint16_t>(std::stoi(line.substr(line.find(':') + 1)));
|
|
|
}
|
|
|
else if (line.find("Collect_Duration_S:") == 0) {
|
|
|
Collect_Duration_S = std::stoi(line.substr(line.find(':') + 1));
|
|
|
}
|
|
|
else if (line.find("Speed_Z:") == 0) {
|
|
|
Speed_Z = std::stod(line.substr(line.find(':') + 1));
|
|
|
}
|
|
|
else if (line.find("Log_Flag:") == 0) {
|
|
|
std::string flag = line.substr(line.find(':') + 1);
|
|
|
Log_Flag = (flag == "YES" || flag == "yes" || flag == "Yes");
|
|
|
}
|
|
|
else if (line.find("Z_Range_MM:") == 0) {
|
|
|
Z_RANGE_MM = std::stod(line.substr(line.find(':') + 1));
|
|
|
}
|
|
|
else if (line.find("Y_Range_MM:") == 0) {
|
|
|
Y_RANGE_MM = std::stod(line.substr(line.find(':') + 1));
|
|
|
}
|
|
|
else if (line.find("Lidar_Hz:") == 0) {
|
|
|
Lidar_Hz = std::stod(line.substr(line.find(':') + 1));
|
|
|
}
|
|
|
else if (line.find("Rect_Width_PX:") == 0) {
|
|
|
Rect_Width_PX = std::stoi(line.substr(line.find(':') + 1));
|
|
|
}
|
|
|
else if (line.find("Rect_Length_Ratio:") == 0) {
|
|
|
Rect_Length_Ratio = std::stod(line.substr(line.find(':') + 1));
|
|
|
}
|
|
|
else if (line.find("Run_Mode:") == 0) {
|
|
|
Run_Mode = trim_str(line.substr(line.find(':') + 1));
|
|
|
}
|
|
|
else if (line.find("Tcp_Port:") == 0) {
|
|
|
Tcp_Port = static_cast<uint16_t>(std::stoi(line.substr(line.find(':') + 1)));
|
|
|
}
|
|
|
|
|
|
else if (line.find("Filter_Polygon:") == 0) {
|
|
|
Filter_Polygon.clear();
|
|
|
std::string data = line.substr(line.find(':') + 1);
|
|
|
// 格式: {-650,0},{-650,670},{650,670},{650,0}
|
|
|
size_t pos = 0;
|
|
|
while (pos < data.size()) {
|
|
|
size_t lb = data.find('{', pos);
|
|
|
if (lb == std::string::npos) break;
|
|
|
size_t rb = data.find('}', lb);
|
|
|
if (rb == std::string::npos) break;
|
|
|
std::string pair = data.substr(lb + 1, rb - lb - 1);
|
|
|
size_t comma = pair.find(',');
|
|
|
if (comma != std::string::npos) {
|
|
|
double x = std::stod(pair.substr(0, comma));
|
|
|
double y = std::stod(pair.substr(comma + 1));
|
|
|
Filter_Polygon.push_back({x, y});
|
|
|
}
|
|
|
pos = rb + 1;
|
|
|
}
|
|
|
Filter_Poly_N = (int)Filter_Polygon.size();
|
|
|
}
|
|
|
else if (line.find("Tuo_Wei:") == 0) {
|
|
|
Tuo_Wei = std::stoi(line.substr(line.find(':') + 1));
|
|
|
if (Tuo_Wei < 0) Tuo_Wei = 0;
|
|
|
if (Tuo_Wei > 3) Tuo_Wei = 3;
|
|
|
}
|
|
|
else if (line.find("Qian_Chong:") == 0) {
|
|
|
Qian_Chong = std::stoi(line.substr(line.find(':') + 1));
|
|
|
if (Qian_Chong < 0) Qian_Chong = 0;
|
|
|
if (Qian_Chong > 3) Qian_Chong = 3;
|
|
|
}
|
|
|
else if (line.find("Yuan_Duan:") == 0) {
|
|
|
Yuan_Duan = std::stoi(line.substr(line.find(':') + 1));
|
|
|
if (Yuan_Duan < 0) Yuan_Duan = 0;
|
|
|
if (Yuan_Duan > 2) Yuan_Duan = 2;
|
|
|
}
|
|
|
else if (line.find("Jin_Duan:") == 0) {
|
|
|
std::string val = line.substr(line.find(':') + 1);
|
|
|
Jin_Duan = (val == "true" || val == "TRUE" || val == "True" ||
|
|
|
val == "yes" || val == "YES" || val == "Yes");
|
|
|
}
|
|
|
else if (line.find("Floor_Height:") == 0) {
|
|
|
Floor_Height = std::stod(line.substr(line.find(':') + 1));
|
|
|
}
|
|
|
else if (line.find("Count_Base_Tolerance:") == 0) {
|
|
|
Count_Base_Tolerance = std::stod(line.substr(line.find(':') + 1));
|
|
|
if (Count_Base_Tolerance < 0) Count_Base_Tolerance = 0;
|
|
|
}
|
|
|
else if (line.find("Count_Additional_Tolerance_Per_Level:") == 0) {
|
|
|
Count_Additional_Tolerance_Per_Level = std::stod(line.substr(line.find(':') + 1));
|
|
|
if (Count_Additional_Tolerance_Per_Level < 0) Count_Additional_Tolerance_Per_Level = 0;
|
|
|
}
|
|
|
else if (line.find("Point_Dilate:") == 0) {
|
|
|
Point_Dilate = std::stoi(line.substr(line.find(':') + 1));
|
|
|
if (Point_Dilate < 1) Point_Dilate = 1;
|
|
|
if (Point_Dilate > 64) Point_Dilate = 64; // 上限保护，防止内存爆炸
|
|
|
}
|
|
|
else if (line.find("Z_Interpolate_Mode:") == 0) {
|
|
|
std::string mode = line.substr(line.find(':') + 1);
|
|
|
// 去掉首尾空白
|
|
|
size_t b = mode.find_first_not_of(" \t\r");
|
|
|
if (b != std::string::npos) mode = mode.substr(b);
|
|
|
size_t e = mode.find_last_not_of(" \t\r");
|
|
|
if (e != std::string::npos) mode = mode.substr(0, e + 1);
|
|
|
if (mode == "CopyTranslation" || mode == "AngleBucketInterpolation")
|
|
|
Z_Interpolate_Mode = mode;
|
|
|
else
|
|
|
Z_Interpolate_Mode = "none";
|
|
|
}
|
|
|
|
|
|
}
|
|
|
fin.close();
|
|
|
}
|
|
|
const double ORIGIN_TOL_MM = 0.5; // mm，判定为原点的容差
|
|
|
inline bool is_origin_point(double x_mm, double y_mm) {
|
|
|
return std::abs(x_mm) < ORIGIN_TOL_MM && std::abs(y_mm) < ORIGIN_TOL_MM;
|
|
|
}
|
|
|
|
|
|
// ==================== 多边形过滤工具 ====================
|
|
|
|
|
|
// 射线法判断点是否在多边形内（支持凹/凸多边形）
|
|
|
// px, py: 点坐标, poly: 顶点数组（与点坐标相同单位）, n: 顶点数
|
|
|
bool point_in_polygon(double px, double py,
|
|
|
const std::vector<std::pair<double, double>>& poly, int n)
|
|
|
{
|
|
|
bool inside = false;
|
|
|
for (int i = 0, j = n - 1; i < n; j = i++)
|
|
|
{
|
|
|
double xi = poly[i].first, yi = poly[i].second;
|
|
|
double xj = poly[j].first, yj = poly[j].second;
|
|
|
|
|
|
if (((yi > py) != (yj > py)) &&
|
|
|
(px < (xj - xi) * (py - yi) / (yj - yi) + xi))
|
|
|
{
|
|
|
inside = !inside;
|
|
|
}
|
|
|
}
|
|
|
return inside;
|
|
|
}
|
|
|
|
|
|
// ==================== 截面切片配置 ====================
|
|
|
// 2D映射图尺寸
|
|
|
const int SLICE_IMG_SIZE = 800; // 输出图片最大像素尺寸
|
|
|
|
|
|
// ==================== PCD 文件保存 ====================
|
|
|
|
|
|
void save_pcd_ascii(const std::string& filename,
|
|
|
const std::vector<double>& xs,
|
|
|
const std::vector<double>& ys,
|
|
|
const std::vector<double>& zs)
|
|
|
{
|
|
|
size_t n = xs.size();
|
|
|
// 一次性构建字符串再写入，避免逐行IO（大量点数时显著提速）
|
|
|
std::ostringstream oss;
|
|
|
oss << "# .PCD v0.7 - Point Cloud Data file format\n"
|
|
|
<< "VERSION 0.7\n"
|
|
|
<< "FIELDS x y z\n"
|
|
|
<< "SIZE 4 4 4\n"
|
|
|
<< "TYPE F F F\n"
|
|
|
<< "COUNT 1 1 1\n"
|
|
|
<< "WIDTH " << n << "\n"
|
|
|
<< "HEIGHT 1\n"
|
|
|
<< "VIEWPOINT 0 0 0 1 0 0 0\n"
|
|
|
<< "POINTS " << n << "\n"
|
|
|
<< "DATA ascii\n"
|
|
|
<< std::fixed << std::setprecision(1);
|
|
|
for (size_t i = 0; i < n; i++) {
|
|
|
oss << xs[i] << " " << ys[i] << " " << zs[i] << "\n";
|
|
|
}
|
|
|
std::string buf = oss.str();
|
|
|
std::ofstream f(filename, std::ios::binary);
|
|
|
f.write(buf.data(), (std::streamsize)buf.size());
|
|
|
f.close();
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// 保存 JPG 灰度图（灰度转RGB后压缩为JPEG，quality=90）
|
|
|
bool save_jpg_gray(const std::string& filename, int width, int height,
|
|
|
const std::vector<uint8_t>& pixels, int quality = 90)
|
|
|
{
|
|
|
// stbi_write_jpg 需要 RGB 三通道数据
|
|
|
std::vector<uint8_t> rgb(width * height * 3);
|
|
|
for (int i = 0; i < width * height; i++) {
|
|
|
uint8_t v = pixels[i];
|
|
|
rgb[i * 3 + 0] = v;
|
|
|
rgb[i * 3 + 1] = v;
|
|
|
rgb[i * 3 + 2] = v;
|
|
|
}
|
|
|
int ret = stbi_write_jpg(filename.c_str(), width, height, 3, rgb.data(), quality);
|
|
|
return ret != 0;
|
|
|
}
|
|
|
|
|
|
// ==================== 连接雷达 ====================
|
|
|
bool connect_lidar(pavo2::pavo2_driver& driver)
|
|
|
{
|
|
|
driver.set_conn_protocol(pavo2::PAVO2_PROTO_TYPE::TCP);
|
|
|
|
|
|
std::cout << "[连接] 正在连接雷达 " << Leida_IP << ":" << Leida_PORT << " ..." << std::endl;
|
|
|
|
|
|
if (!driver.pavo2_open(Leida_IP, Leida_PORT, OPEN_TIMEOUT_MS))
|
|
|
{
|
|
|
std::cerr << "[错误] 连接雷达失败！请检查IP和网络连接。" << std::endl;
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
std::cout << "[连接] 雷达连接成功！" << std::endl;
|
|
|
|
|
|
driver.enable_tail_filter(Tuo_Wei);
|
|
|
driver.enable_front_filter(Qian_Chong);
|
|
|
driver.enable_far_hr_filter(Yuan_Duan);
|
|
|
driver.enable_near_hr(Jin_Duan);
|
|
|
|
|
|
|
|
|
std::string sn, pn, sdk_ver, fw_ver;
|
|
|
if (driver.get_device_sn(sn))
|
|
|
std::cout << "[信息] 设备SN: " << sn << std::endl;
|
|
|
if (driver.get_device_pn(pn))
|
|
|
std::cout << "[信息] 设备PN: " << pn << std::endl;
|
|
|
if (driver.get_sdk_ver(sdk_ver))
|
|
|
std::cout << "[信息] SDK版本: " << sdk_ver << std::endl;
|
|
|
if (driver.get_firmware_ver(fw_ver))
|
|
|
std::cout << "[信息] 固件版本: " << fw_ver << std::endl;
|
|
|
|
|
|
if (!driver.enable_data(true))
|
|
|
{
|
|
|
std::cerr << "[错误] 开启数据传输失败!" << std::endl;
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
std::cout << "[信息] 雷达数据传输已开启。" << std::endl;
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
// ==================== 功能1：单帧 + 鸟瞰图 ====================
|
|
|
void demo_single_frame(pavo2::pavo2_driver& driver)
|
|
|
{
|
|
|
std::cout << "\n========== 功能1：获取单帧雷达数据 ==========" << std::endl;
|
|
|
|
|
|
std::vector<pavo2::pavo_response_pcd_t> points;
|
|
|
uint32_t timestamp = 0;
|
|
|
|
|
|
if (!driver.get_scanned_data(points, timestamp, FRAME_TIMEOUT_MS))
|
|
|
{
|
|
|
std::cerr << "[错误] 获取单帧数据超时或失败！" << std::endl;
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
std::cout << "[结果] 成功获取一帧数据：" << std::endl;
|
|
|
std::cout << " 原始点数量: " << points.size() << std::endl;
|
|
|
|
|
|
if (points.empty()) return;
|
|
|
|
|
|
// DEBUG: 检查原始坐标范围 + Y轴偏移计算
|
|
|
double y_offset_single = 0.0; // mm
|
|
|
{
|
|
|
double xmin = 1e9, xmax = -1e9, ymin = 1e9, ymax = -1e9;
|
|
|
int neg_y = 0, pos_y = 0;
|
|
|
for (const auto& p : points) {
|
|
|
double x_mm = p.x * 2.0, y_mm = p.y * 2.0;
|
|
|
if (x_mm < xmin) xmin = x_mm; if (x_mm > xmax) xmax = x_mm;
|
|
|
if (y_mm < ymin) ymin = y_mm; if (y_mm > ymax) ymax = y_mm;
|
|
|
if (y_mm < -0.5) neg_y++; if (y_mm > 0.5) pos_y++;
|
|
|
}
|
|
|
std::cout << "[DEBUG单帧] X范围: " << xmin << " ~ " << xmax << " mm" << std::endl;
|
|
|
std::cout << "[DEBUG单帧] Y范围: " << ymin << " ~ " << ymax << " mm" << std::endl;
|
|
|
std::cout << "[DEBUG单帧] Y>0点数: " << pos_y << " Y<0点数: " << neg_y << std::endl;
|
|
|
if (SHIFT_Y_POSITIVE && ymin < 0) {
|
|
|
y_offset_single = -ymin;
|
|
|
std::cout << "[DEBUG单帧] Y轴偏移: +" << y_offset_single << " mm (SHIFT_Y_POSITIVE=true)" << std::endl;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// --- 鸟瞰图参数 ---
|
|
|
const int IMG_SIZE = 800; // 图片像素尺寸
|
|
|
const double RANGE_MM = 10000.0; // ±10m 范围
|
|
|
const double SCALE = IMG_SIZE / (2.0 * RANGE_MM); // mm转像素
|
|
|
|
|
|
// 多边形过滤 + 原点过滤
|
|
|
int filtered_count = 0;
|
|
|
int origin_skip = 0;
|
|
|
std::vector<uint8_t> image(IMG_SIZE * IMG_SIZE, 0);
|
|
|
|
|
|
for (const auto& p : points)
|
|
|
{
|
|
|
double x_mm = p.x * 2.0; // 原始单位0.002m，转mm
|
|
|
double y_mm = p.y * 2.0 + y_offset_single; // Y轴上移
|
|
|
|
|
|
// 跳过原点无效点
|
|
|
if (is_origin_point(x_mm, y_mm)) {
|
|
|
origin_skip++;
|
|
|
continue;
|
|
|
}
|
|
|
|
|
|
if (FILTER_ENABLE && !point_in_polygon(x_mm, y_mm, Filter_Polygon, Filter_Poly_N))
|
|
|
continue;
|
|
|
|
|
|
|
|
|
filtered_count++;
|
|
|
|
|
|
// 世界坐标 -> 像素坐标（图片中心为原点，Y轴翻转）
|
|
|
int px = (int)((x_mm + RANGE_MM) * SCALE);
|
|
|
int py = (int)((RANGE_MM - y_mm) * SCALE); // 上方向为Y正
|
|
|
|
|
|
if (px >= 0 && px < IMG_SIZE && py >= 0 && py < IMG_SIZE)
|
|
|
{
|
|
|
// 亮度 = intensity，叠加时取最大值
|
|
|
int idx = py * IMG_SIZE + px;
|
|
|
uint8_t val = (uint8_t)(std::min(p.intensity, (uint16_t)255));
|
|
|
if (val > image[idx]) image[idx] = val;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 标记雷达中心点 (小十字)
|
|
|
int cx = IMG_SIZE / 2, cy = IMG_SIZE / 2;
|
|
|
for (int dx = -3; dx <= 3; dx++) {
|
|
|
int idx = cy * IMG_SIZE + (cx + dx);
|
|
|
if (idx >= 0 && idx < IMG_SIZE * IMG_SIZE) image[idx] = 255;
|
|
|
}
|
|
|
for (int dy = -3; dy <= 3; dy++) {
|
|
|
int idx = (cy + dy) * IMG_SIZE + cx;
|
|
|
if (idx >= 0 && idx < IMG_SIZE * IMG_SIZE) image[idx] = 255;
|
|
|
}
|
|
|
|
|
|
std::string img_file = "single_frame.jpg";
|
|
|
if (save_jpg_gray(img_file, IMG_SIZE, IMG_SIZE, image))
|
|
|
std::cout << "[图像] 鸟瞰图已保存: " << img_file
|
|
|
<< " (" << IMG_SIZE << "x" << IMG_SIZE << ", 范围±" << RANGE_MM/1000 << "m, JPG)" << std::endl;
|
|
|
|
|
|
// 点云也保存为 txt（仅过滤区域内的点）
|
|
|
std::ofstream txt("single_frame.txt");
|
|
|
txt << "# 过滤区域: x∈[-10,10]m, y∈[10,20]m (FILTER_POLY_N=" << Filter_Poly_N << ")\n";
|
|
|
txt << "# x(mm) y(mm) intensity\n";
|
|
|
for (const auto& p : points)
|
|
|
{
|
|
|
double x_mm = p.x * 2.0;
|
|
|
double y_mm = p.y * 2.0 + y_offset_single; // Y轴上移
|
|
|
if (is_origin_point(x_mm, y_mm)) continue;
|
|
|
if (FILTER_ENABLE && !point_in_polygon(x_mm, y_mm, Filter_Polygon, Filter_Poly_N))
|
|
|
continue;
|
|
|
txt << std::fixed << std::setprecision(1)
|
|
|
<< x_mm << " " << y_mm << " " << p.intensity << "\n";
|
|
|
}
|
|
|
txt.close();
|
|
|
std::cout << "[过滤] 区域内点数: " << filtered_count << " / " << points.size() << std::endl;
|
|
|
if (origin_skip > 0)
|
|
|
std::cout << "[过滤] 原点无效点跳过: " << origin_skip << " 点" << std::endl;
|
|
|
std::cout << "[文件] 点云数据已保存: single_frame.txt" << std::endl;
|
|
|
}
|
|
|
|
|
|
// 一次检测的结果（供TCP返回）
|
|
|
struct DetectResult
|
|
|
{
|
|
|
double start_fill_pct = -1.0; // 起点截面白色占比(%)
|
|
|
double end_fill_pct = -1.0; // 终点截面白色占比(%)
|
|
|
double start_y_max = 0.0; // 起点截面Y最大值(mm)
|
|
|
double start_y_med = 0.0; // 起点截面Y中位数(mm)
|
|
|
double end_y_max = 0.0; // 终点截面Y最大值(mm)
|
|
|
double end_y_med = 0.0; // 终点截面Y中位数(mm)
|
|
|
bool valid = false; // 是否有有效结果
|
|
|
};
|
|
|
|
|
|
// ==================== 功能2：多秒采集 + 深度图 ====================
|
|
|
void demo_collect_multi(pavo2::pavo2_driver& driver, DetectResult* result = nullptr)
|
|
|
{
|
|
|
std::cout << "\n========== 功能2：采集 " << Collect_Duration_S
|
|
|
<< " 秒雷达数据 ==========" << std::endl;
|
|
|
|
|
|
// --- 创建输出目录(data/当前时间 年月日HH-MM-SS) + 日志文件 ---
|
|
|
time_t rawtime = std::time(nullptr);
|
|
|
struct tm* timeinfo = std::localtime(&rawtime);
|
|
|
char dir_buf[32];
|
|
|
//std::strftime(dir_buf, sizeof(dir_buf), "%H-%M-%S", timeinfo);
|
|
|
std::strftime(dir_buf, sizeof(dir_buf), "data/%Y%m%d/%H%M%S", timeinfo); //文件放到data下面好管理
|
|
|
std::string output_dir(dir_buf);
|
|
|
mkdir("data");
|
|
|
mkdir(output_dir.substr(0, output_dir.rfind('/')).c_str());
|
|
|
_mkdir(output_dir.c_str());
|
|
|
//增加日志使能
|
|
|
std::ofstream log_file;
|
|
|
if (Log_Flag) {
|
|
|
log_file.open(output_dir + "/log.txt");
|
|
|
}
|
|
|
//std::ofstream log_file(output_dir + "/log.txt");
|
|
|
auto log_msg = [&log_file](const std::string& s) {
|
|
|
std::cout << s;
|
|
|
if (log_file.is_open()) log_file << gbk_to_utf8(s);
|
|
|
};
|
|
|
log_msg("[目录] 输出目录: " + output_dir + "/\n");
|
|
|
|
|
|
struct PointRecord
|
|
|
{
|
|
|
double x_mm;
|
|
|
double y_mm;
|
|
|
double angle_deg; // 角度(度)
|
|
|
uint16_t distance; // 原始距离(unit=2mm)
|
|
|
double z_val; // Z轴值: 距离(mm) 或 时间(ms)，取决于Speed_Z
|
|
|
};
|
|
|
|
|
|
// 帧数据（用于深度图）
|
|
|
struct FrameData
|
|
|
{
|
|
|
uint32_t elapsed_ms;
|
|
|
std::vector<uint16_t> dist_by_angle; // 每个角度bin的距离
|
|
|
std::vector<uint8_t> intensity_by_angle;
|
|
|
};
|
|
|
|
|
|
std::vector<PointRecord> all_points;
|
|
|
std::vector<FrameData> all_frames;
|
|
|
size_t origin_skip_total = 0;
|
|
|
|
|
|
// 清空SDK缓冲帧：enable_data后等待稳定期间，网络线程会把完整扫描帧缓存进队列，
|
|
|
// 若不清空，采集开始时会瞬间连续取出这些旧帧，它们的Z值几乎都为0，全部堆在Z起点。
|
|
|
int drained = 0;
|
|
|
{
|
|
|
pavo2::pavo_packet_info_scan_t tmp_scan;
|
|
|
while (driver.get_scanned_data(tmp_scan, 5)) // 5ms超时，空队列立即结束
|
|
|
drained++;
|
|
|
}
|
|
|
if (drained > 0)
|
|
|
std::cout << "[采集] 已清空缓冲旧帧 " << drained << " 帧" << std::endl;
|
|
|
|
|
|
auto start_time = std::chrono::steady_clock::now();
|
|
|
int frame_count = 0;
|
|
|
size_t total_points = 0;
|
|
|
|
|
|
// 深度图参数
|
|
|
const int DEPTH_ANGLE_BINS = 720; // 0.5度一个bin
|
|
|
|
|
|
std::cout << "[采集] 开始采集，将持续 " << Collect_Duration_S << " 秒..." << std::endl;
|
|
|
if (Speed_Z > 0)
|
|
|
std::cout << "[采集] z轴 = 距离(mm), 速度=" << Speed_Z << " cm/s" << std::endl;
|
|
|
else
|
|
|
std::cout << "[采集] z轴 = 从采集开始的系统毫秒时间" << std::endl;
|
|
|
|
|
|
while (true)
|
|
|
{
|
|
|
auto now = std::chrono::steady_clock::now();
|
|
|
auto elapsed_s = std::chrono::duration_cast<std::chrono::seconds>(now - start_time).count();
|
|
|
auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - start_time).count();
|
|
|
|
|
|
if (elapsed_s >= Collect_Duration_S)
|
|
|
{
|
|
|
std::cout << "[采集] " << Collect_Duration_S << " 秒到，停止采集。" << std::endl;
|
|
|
break;
|
|
|
}
|
|
|
|
|
|
// 获取原始scan数据（带角度），便于做深度图
|
|
|
pavo2::pavo_packet_info_scan_t scan_info;
|
|
|
if (driver.get_scanned_data(scan_info, FRAME_TIMEOUT_MS))
|
|
|
{
|
|
|
frame_count++;
|
|
|
// Z轴：距离模式 或 时间模式
|
|
|
double z_val = (Speed_Z > 0)
|
|
|
? (Speed_Z * 10.0 * elapsed_ms / 1000.0) // cm/s → mm/s, × ms = mm
|
|
|
: (double)elapsed_ms;
|
|
|
|
|
|
// 构建本帧的角度-距离直方图
|
|
|
FrameData frame;
|
|
|
frame.elapsed_ms = (uint32_t)elapsed_ms;
|
|
|
frame.dist_by_angle.assign(DEPTH_ANGLE_BINS, 0);
|
|
|
frame.intensity_by_angle.assign(DEPTH_ANGLE_BINS, 0);
|
|
|
|
|
|
for (const auto& s : scan_info.vec_buff)
|
|
|
{
|
|
|
double angle_deg = s.angle / 100.0; // SDK角度单位是0.01度
|
|
|
// 雷达0°指向-Y方向(第3/4象限交界)，旋转-90°映射：
|
|
|
// cos(A-90°)=sin(A), sin(A-90°)=-cos(A)
|
|
|
// 0°→(0,-dist)向下, 90°→(+dist,0)向右, 180°→(0,+dist)向上, 270°→(-dist,0)向左
|
|
|
double angle_rad = angle_deg * 3.14159265 / 180.0;
|
|
|
double x_mm = s.distance * sin(angle_rad) * 2.0;
|
|
|
double y_mm = -s.distance * cos(angle_rad) * 2.0;
|
|
|
|
|
|
// 跳过原点无效点
|
|
|
if (is_origin_point(x_mm, y_mm)) {
|
|
|
origin_skip_total++;
|
|
|
continue;
|
|
|
}
|
|
|
|
|
|
// 多边形过滤：只保留区域内的点（FILTER_POLY_N=0 则不过滤）
|
|
|
if (FILTER_ENABLE && !point_in_polygon(x_mm, y_mm, Filter_Polygon, Filter_Poly_N))
|
|
|
continue;
|
|
|
|
|
|
all_points.push_back({x_mm, y_mm, angle_deg, s.distance, z_val});
|
|
|
|
|
|
// 角度 -> bin
|
|
|
int bin = (int)(angle_deg / 360.0 * DEPTH_ANGLE_BINS + 0.5) % DEPTH_ANGLE_BINS;
|
|
|
if (s.distance > frame.dist_by_angle[bin])
|
|
|
frame.dist_by_angle[bin] = s.distance;
|
|
|
if (s.intensity > frame.intensity_by_angle[bin])
|
|
|
frame.intensity_by_angle[bin] = (uint8_t)(std::min(s.intensity, (uint16_t)255));
|
|
|
}
|
|
|
|
|
|
all_frames.push_back(frame);
|
|
|
total_points += scan_info.vec_buff.size();
|
|
|
|
|
|
if (frame_count % 50 == 0)
|
|
|
{
|
|
|
std::cout << " 已采集 " << elapsed_s << "s, 帧数=" << frame_count
|
|
|
<< ", 总点数=" << total_points << std::endl;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
std::cout << "[采集] 采集完成！" << std::endl;
|
|
|
std::cout << " 总帧数: " << frame_count << std::endl;
|
|
|
std::cout << " 总点数: " << total_points << std::endl;
|
|
|
if (origin_skip_total > 0)
|
|
|
std::cout << " 原点无效点跳过: " << origin_skip_total << " 点" << std::endl;
|
|
|
|
|
|
// ==========> 保存多边形过滤后的原始PCD（Y偏移之前）<==========
|
|
|
if (!all_points.empty()) {
|
|
|
std::vector<double> raw_xs, raw_ys, raw_zs;
|
|
|
raw_xs.reserve(all_points.size());
|
|
|
raw_ys.reserve(all_points.size());
|
|
|
raw_zs.reserve(all_points.size());
|
|
|
for (const auto& pt : all_points) {
|
|
|
raw_xs.push_back(pt.x_mm);
|
|
|
raw_ys.push_back(pt.y_mm);
|
|
|
raw_zs.push_back(pt.z_val);
|
|
|
}
|
|
|
save_pcd_ascii(output_dir + "/lidar_data.pcd", raw_xs, raw_ys, raw_zs);
|
|
|
std::cout << "[文件] 多边形过滤后原始点云已保存: lidar_data.pcd ("
|
|
|
<< all_points.size() << " 点, Y未偏移)" << std::endl;
|
|
|
}
|
|
|
|
|
|
// Y轴偏移
|
|
|
if (!all_points.empty()) {
|
|
|
double ymin = 1e9;
|
|
|
for (const auto& pt : all_points) {
|
|
|
if (pt.y_mm < ymin) ymin = pt.y_mm;
|
|
|
}
|
|
|
if (SHIFT_Y_POSITIVE && ymin < 0) {
|
|
|
double y_offset = -ymin;
|
|
|
for (auto& pt : all_points) {
|
|
|
pt.y_mm += y_offset;
|
|
|
}
|
|
|
std::cout << "[信息] Y轴偏移: +" << y_offset << " mm" << std::endl;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// ==========> 保存Y偏移后的PCD <==========
|
|
|
if (!all_points.empty()) {
|
|
|
std::vector<double> shifted_xs, shifted_ys, shifted_zs;
|
|
|
shifted_xs.reserve(all_points.size());
|
|
|
shifted_ys.reserve(all_points.size());
|
|
|
shifted_zs.reserve(all_points.size());
|
|
|
for (const auto& pt : all_points) {
|
|
|
shifted_xs.push_back(pt.x_mm);
|
|
|
shifted_ys.push_back(pt.y_mm);
|
|
|
shifted_zs.push_back(pt.z_val);
|
|
|
}
|
|
|
save_pcd_ascii(output_dir + "/lidar_data_shifted.pcd", shifted_xs, shifted_ys, shifted_zs);
|
|
|
std::cout << "[文件] Y偏移后点云已保存: lidar_data_shifted.pcd ("
|
|
|
<< all_points.size() << " 点, Y已偏移)" << std::endl;
|
|
|
}
|
|
|
|
|
|
// ============ 截面切片：Y轴切片 → X-Z平面映射 (起点+终点各一张) ============
|
|
|
if (all_points.empty()) {
|
|
|
if (Log_Flag && log_file.is_open())//0713改
|
|
|
log_file << gbk_to_utf8("无点云数据，跳过截面切片。") << std::endl;
|
|
|
log_file.close();
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
// Step 1: Y范围（全图）
|
|
|
double y_min = all_points[0].y_mm;
|
|
|
double y_max = all_points[0].y_mm;
|
|
|
for (const auto& pt : all_points) {
|
|
|
if (pt.y_mm < y_min) y_min = pt.y_mm;
|
|
|
if (pt.y_mm > y_max) y_max = pt.y_mm;
|
|
|
}
|
|
|
std::cout << "\n[截面] Y全图范围: " << y_min << " ~ " << y_max << " mm" << std::endl;
|
|
|
|
|
|
// Step 2: Y截面区间 = [Y最小, Y最小+Y_Range_MM]，只取该区间内的点参与
|
|
|
double y_slice_max = y_min + Y_RANGE_MM;
|
|
|
std::vector<const PointRecord*> y_filtered;
|
|
|
y_filtered.reserve(all_points.size());
|
|
|
for (const auto& pt : all_points) {
|
|
|
if (pt.y_mm >= y_min && pt.y_mm <= y_slice_max)
|
|
|
y_filtered.push_back(&pt);
|
|
|
}
|
|
|
std::cout << "[截面] Y截面区间: " << y_min << " ~ " << y_slice_max
|
|
|
<< " mm (长度=" << Y_RANGE_MM << "mm, 配置Y_Range_MM)" << std::endl;
|
|
|
std::cout << "[截面] 参与点数: " << y_filtered.size() << " / " << all_points.size() << std::endl;
|
|
|
if (y_filtered.empty()) {
|
|
|
std::cout << "[截面] Y截面区间内无点，跳过截面生成。" << std::endl;
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
// Step 3: 计算全局X范围和Z范围
|
|
|
// X范围: 用全图所有点的X最大~X最小（图像高度方向，必须全图范围）
|
|
|
// Z范围: 用Y过滤后点的Z最大~Z最小（定义起终点截面区间用）
|
|
|
double gx_min = all_points[0].x_mm, gx_max = all_points[0].x_mm;
|
|
|
for (const auto& pt : all_points) {
|
|
|
if (pt.x_mm < gx_min) gx_min = pt.x_mm;
|
|
|
if (pt.x_mm > gx_max) gx_max = pt.x_mm;
|
|
|
}
|
|
|
double gz_min = y_filtered[0]->z_val, gz_max = y_filtered[0]->z_val;
|
|
|
for (const auto* pt : y_filtered) {
|
|
|
if (pt->z_val < gz_min) gz_min = pt->z_val;
|
|
|
if (pt->z_val > gz_max) gz_max = pt->z_val;
|
|
|
}
|
|
|
const char* z_unit = (Speed_Z > 0) ? "mm" : "ms";
|
|
|
std::cout << "[截面] 全局X范围(全图点云): " << gx_min << " ~ " << gx_max << " mm (图像高度方向)" << std::endl;
|
|
|
std::cout << "[截面] 全局Z范围: " << gz_min << " ~ " << gz_max << " " << z_unit << std::endl;
|
|
|
std::cout << "[截面] 每张图高度由全图 X最大~X最小 范围决定(不再固定800px)" << std::endl;
|
|
|
|
|
|
// Step 4: 两个Z区间
|
|
|
double z_first_start = gz_min;
|
|
|
double z_first_end = gz_min + Z_RANGE_MM;
|
|
|
double z_last_end = gz_max;
|
|
|
double z_last_start = gz_max - Z_RANGE_MM;
|
|
|
if (z_first_end > gz_max) z_first_end = gz_max;
|
|
|
if (z_last_start < gz_min) z_last_start = gz_min;
|
|
|
|
|
|
// ====== 通用：Y+Z过滤 → X-Z平面二值映射 ======
|
|
|
auto make_xz_slice = [&](double z0, double z1, const std::string& label,
|
|
|
const std::string& out_pcd, const std::string& out_jpg,
|
|
|
double* out_fill_pct = nullptr,
|
|
|
int* out_w = nullptr, int* out_h = nullptr,
|
|
|
double* out_y_max = nullptr, double* out_y_med = nullptr,
|
|
|
std::vector<uint8_t>* out_img = nullptr,
|
|
|
bool right_align = false)
|
|
|
{
|
|
|
std::ostringstream oss;
|
|
|
oss << "\n[截面-" << label << "] Z区间: " << z0 << " ~ " << z1 << " " << z_unit << "\n";
|
|
|
log_msg(oss.str());
|
|
|
|
|
|
// 收集切片点（Y已过滤 + Z区间过滤）
|
|
|
std::vector<double> slice_xs, slice_ys, slice_zs;
|
|
|
for (const auto* pt : y_filtered) {
|
|
|
if (pt->z_val >= z0 && pt->z_val <= z1) {
|
|
|
slice_xs.push_back(pt->x_mm);
|
|
|
slice_ys.push_back(pt->y_mm);
|
|
|
slice_zs.push_back(pt->z_val);
|
|
|
}
|
|
|
}
|
|
|
oss.str(""); oss.clear();
|
|
|
oss << "[截面-" << label << "] 切片点数: " << slice_xs.size() << "\n";
|
|
|
log_msg(oss.str());
|
|
|
|
|
|
if (slice_xs.empty()) {
|
|
|
oss.str(""); oss.clear();
|
|
|
oss << "[截面-" << label << "] 无数据，跳过。\n";
|
|
|
log_msg(oss.str());
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
// 图像高度方向采用全图 X 范围（gx_min~gx_max），高度=该范围/比例（不再固定800px）
|
|
|
double sx_min = gx_min, sx_max = gx_max;
|
|
|
double sx_range = sx_max - sx_min;
|
|
|
if (sx_range < 1.0) sx_range = 1.0;
|
|
|
// 图像尺寸：宽=Z_Range_MM(px)，高=全图X范围(px)，1px=1mm，不做比例缩放
|
|
|
const int img_w = std::max(1, (int)std::ceil(Z_RANGE_MM)); // 宽 = Z_Range_MM
|
|
|
const double img_scale = 1.0; // 1px = 1mm
|
|
|
int img_h = std::max(1, (int)std::ceil(sx_range)); // 高 = 全图X范围
|
|
|
if (out_w) *out_w = img_w;
|
|
|
if (out_h) *out_h = img_h;
|
|
|
|
|
|
oss.str(""); oss.clear();
|
|
|
oss << "[截面-" << label << "] 高度方向X范围(全图): " << gx_min << " ~ " << gx_max
|
|
|
<< " mm\n";
|
|
|
log_msg(oss.str());
|
|
|
oss.str(""); oss.clear();
|
|
|
oss << "[截面-" << label << "] 图像尺寸: " << img_w << "x" << img_h
|
|
|
<< " px (1px=1mm: 宽=Z_Range_MM=" << Z_RANGE_MM
|
|
|
<< ", 高=全图X范围=" << sx_range << ")\n";
|
|
|
log_msg(oss.str());
|
|
|
|
|
|
// Y统计: 最大值 + 中位数
|
|
|
double y_max_slice = *std::max_element(slice_ys.begin(), slice_ys.end());
|
|
|
std::vector<double> ys_sorted = slice_ys;
|
|
|
std::sort(ys_sorted.begin(), ys_sorted.end());
|
|
|
double y_med = (ys_sorted.size() % 2 == 1)
|
|
|
? ys_sorted[ys_sorted.size() / 2]
|
|
|
: (ys_sorted[ys_sorted.size() / 2 - 1] + ys_sorted[ys_sorted.size() / 2]) / 2.0;
|
|
|
oss.str(""); oss.clear();
|
|
|
oss << "[截面-" << label << "] Y统计: 最大值=" << std::fixed << std::setprecision(1)
|
|
|
<< y_max_slice << " mm, 中位数=" << y_med << " mm\n";
|
|
|
log_msg(oss.str());
|
|
|
if (out_y_max) *out_y_max = y_max_slice;
|
|
|
if (out_y_med) *out_y_med = y_med;
|
|
|
|
|
|
// 保存 PCD
|
|
|
save_pcd_ascii(out_pcd, slice_xs, slice_ys, slice_zs);
|
|
|
oss.str(""); oss.clear();
|
|
|
oss << "[文件] " << out_pcd << " (" << slice_xs.size() << " 点)\n";
|
|
|
log_msg(oss.str());
|
|
|
|
|
|
// X-Z平面映射：矩形渲染
|
|
|
// 横轴=Z(运动方向, 左近→右远), 纵轴=X(X越大越靠上)
|
|
|
// 每个点作为矩形的左边中心点，矩形向右延伸(长=速度/帧率=每帧移动距离)，宽=Rect_Width_PX
|
|
|
std::vector<uint8_t> slice_img(img_w * img_h, 0);
|
|
|
|
|
|
// 矩形长(mm) = 运行速度(mm/s) / 雷达帧率(Hz) × 倍率(默认1.5)
|
|
|
double rect_len_mm = 10.0; // 时间模式(无速度)默认10mm
|
|
|
if (Speed_Z > 0)
|
|
|
rect_len_mm = Speed_Z * 10.0 / Lidar_Hz * Rect_Length_Ratio;
|
|
|
int len_px = std::max(1, (int)(rect_len_mm / img_scale));
|
|
|
int half_w = Rect_Width_PX / 2;
|
|
|
if (half_w < 1) half_w = 1;
|
|
|
|
|
|
// 水平方向定位：start左对齐(矩形紧贴左边缘)，end右对齐(矩形紧贴右边缘)
|
|
|
// 切片Z区间长度==图像宽度(1px=1mm)，矩形铺满整图，左右不留黑边
|
|
|
double z_ref = z0;
|
|
|
int x_offset = 0; // start：最左点的矩形左边缘=0
|
|
|
if (right_align && !slice_zs.empty()) {
|
|
|
z_ref = *std::max_element(slice_zs.begin(), slice_zs.end());
|
|
|
x_offset = img_w - len_px; // end：最右点的矩形右边缘=图宽-1
|
|
|
}
|
|
|
|
|
|
for (size_t i = 0; i < slice_xs.size(); i++) {
|
|
|
int cx = right_align
|
|
|
? (int)(x_offset - (z_ref - slice_zs[i]) / img_scale) // 右对齐：右侧中间
|
|
|
: (int)((slice_zs[i] - z0) / img_scale + x_offset); // 左对齐：左侧中间
|
|
|
int cy = (int)((sx_max - slice_xs[i]) / img_scale); // 矩形左边中心点y
|
|
|
|
|
|
// 以 (cx, cy) 为左边中心画矩形: 向右延伸 len_px，上下各 half_w
|
|
|
for (int dx = 0; dx < len_px; dx++) {
|
|
|
for (int dy = -half_w; dy <= half_w; dy++) {
|
|
|
int px = cx + dx;
|
|
|
int py = cy + dy;
|
|
|
if (px >= 0 && px < img_w && py >= 0 && py < img_h) {
|
|
|
slice_img[py * img_w + px] = 255;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 统计矩形覆盖像素（白色）的占比
|
|
|
int filled = 0;
|
|
|
for (int i = 0; i < img_w * img_h; i++)
|
|
|
if (slice_img[i] > 0) filled++;
|
|
|
double fill_pct = (img_w * img_h > 0)
|
|
|
? filled * 100.0 / (img_w * img_h) : 0.0;
|
|
|
if (out_fill_pct) *out_fill_pct = fill_pct;
|
|
|
|
|
|
// 白色(所有矩形)占全图比例日志：无论图片保存成功与否都输出
|
|
|
oss.str(""); oss.clear();
|
|
|
oss << "[截面-" << label << "] 白色占比: " << filled << "/"
|
|
|
<< (img_w * img_h) << " 像素 ("
|
|
|
<< std::fixed << std::setprecision(2) << fill_pct << "%)\n";
|
|
|
log_msg(oss.str());
|
|
|
|
|
|
if (save_jpg_gray(out_jpg, img_w, img_h, slice_img)) {
|
|
|
oss.str(""); oss.clear();
|
|
|
oss << "[图像] " << out_jpg << " (" << img_w << "x" << img_h
|
|
|
<< ", Z=" << z0 << "~" << z1 << " " << z_unit
|
|
|
<< ", X=" << sx_min << "~" << sx_max << "mm"
|
|
|
<< ", 矩形长=" << rect_len_mm << "mm(" << len_px << "px)"
|
|
|
<< ", 矩形宽=" << Rect_Width_PX << "px"
|
|
|
<< ", 对齐=" << (right_align ? "右" : "左") << ")\n";
|
|
|
log_msg(oss.str());
|
|
|
}
|
|
|
|
|
|
if (out_img) *out_img = slice_img; // 回传渲染图，用于合并对比图
|
|
|
};
|
|
|
|
|
|
double start_fill_pct = -1.0, end_fill_pct = -1.0;
|
|
|
int start_w = 0, start_h = 0, end_w = 0, end_h = 0;
|
|
|
double start_y_max = 0.0, start_y_med = 0.0, end_y_max = 0.0, end_y_med = 0.0;
|
|
|
std::vector<uint8_t> start_img, end_img;
|
|
|
|
|
|
// 起点截面
|
|
|
make_xz_slice(z_first_start, z_first_end, "起点",
|
|
|
output_dir + "/lidar_slice_start.pcd",
|
|
|
output_dir + "/lidar_slice_start.jpg",
|
|
|
&start_fill_pct, &start_w, &start_h,
|
|
|
&start_y_max, &start_y_med, &start_img,
|
|
|
false); // 起点：左对齐，在左侧
|
|
|
|
|
|
// 终点截面（结束前 Z_RANGE_MM）
|
|
|
if (z_last_start < z_last_end) {
|
|
|
make_xz_slice(z_last_start, z_last_end, "终点",
|
|
|
output_dir + "/lidar_slice_end.pcd",
|
|
|
output_dir + "/lidar_slice_end.jpg",
|
|
|
&end_fill_pct, &end_w, &end_h,
|
|
|
&end_y_max, &end_y_med, &end_img,
|
|
|
true); // 终点：右对齐，在右侧
|
|
|
} else {
|
|
|
std::cout << "\n[截面-终点] Z总长不足" << Z_RANGE_MM << "mm，跳过终点截面。" << std::endl;
|
|
|
}
|
|
|
|
|
|
// ====== 合并对比图：start在左半区，end在右半区 ======
|
|
|
if (!start_img.empty() && !end_img.empty() && start_w == end_w && start_h == end_h) {
|
|
|
int comb_w = start_w * 2;
|
|
|
int comb_h = start_h;
|
|
|
std::vector<uint8_t> combined(comb_w * comb_h, 0);
|
|
|
for (int y = 0; y < comb_h; y++) {
|
|
|
std::memcpy(&combined[y * comb_w], &start_img[y * start_w], start_w);
|
|
|
std::memcpy(&combined[y * comb_w + start_w], &end_img[y * start_w], start_w);
|
|
|
combined[y * comb_w + start_w] = 128; // 中间分隔线(灰)
|
|
|
}
|
|
|
if (save_jpg_gray(output_dir + "/lidar_slice_combined.jpg", comb_w, comb_h, combined)) {
|
|
|
std::cout << "[图像] " << output_dir << "/lidar_slice_combined.jpg ("
|
|
|
<< comb_w << "x" << comb_h << ", 左=start 右=end)" << std::endl;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 回传检测结果（供TCP返回）
|
|
|
if (result) {
|
|
|
result->valid = true;
|
|
|
result->start_fill_pct = start_fill_pct;
|
|
|
result->end_fill_pct = end_fill_pct;
|
|
|
result->start_y_max = start_y_max;
|
|
|
result->start_y_med = start_y_med;
|
|
|
result->end_y_max = end_y_max;
|
|
|
result->end_y_med = end_y_med;
|
|
|
}
|
|
|
|
|
|
// --- 写入日志摘要 ---
|
|
|
{
|
|
|
std::ostringstream sum;
|
|
|
sum << "\n========== 采集摘要 ==========" << std::endl;
|
|
|
sum << "采集时长: " << Collect_Duration_S << " s" << std::endl;
|
|
|
sum << "总帧数: " << frame_count << std::endl;
|
|
|
sum << "总点数: " << total_points << std::endl;
|
|
|
sum << "过滤后点数: " << all_points.size() << std::endl;
|
|
|
sum << "原点无效点跳过: " << origin_skip_total << std::endl;
|
|
|
sum << "Y范围(全部点参与): " << y_min << " ~ " << y_max << " mm" << std::endl;
|
|
|
sum << "参与点数: " << y_filtered.size() << std::endl;
|
|
|
sum << "Z截面区间长度: " << Z_RANGE_MM << " mm" << std::endl;
|
|
|
sum << "雷达帧率: " << Lidar_Hz << " Hz" << std::endl;
|
|
|
sum << "矩形宽度: " << Rect_Width_PX << " px" << std::endl;
|
|
|
sum << "全局X范围: " << gx_min << " ~ " << gx_max << " mm" << std::endl;
|
|
|
sum << "全局Z范围: " << gz_min << " ~ " << gz_max << " " << z_unit << std::endl;
|
|
|
if (start_w > 0)
|
|
|
sum << "起点截面图像尺寸: " << start_w << "x" << start_h << " px" << std::endl;
|
|
|
if (end_w > 0)
|
|
|
sum << "终点截面图像尺寸: " << end_w << "x" << end_h << " px" << std::endl;
|
|
|
if (start_fill_pct >= 0.0)
|
|
|
sum << "起点截面白色占比: " << std::fixed << std::setprecision(2)
|
|
|
<< start_fill_pct << "%" << std::endl;
|
|
|
if (end_fill_pct >= 0.0)
|
|
|
sum << "终点截面白色占比: " << std::fixed << std::setprecision(2)
|
|
|
<< end_fill_pct << "%" << std::endl;
|
|
|
sum << "输出文件:" << std::endl;
|
|
|
sum << " " << output_dir << "/lidar_data.pcd" << std::endl;
|
|
|
sum << " " << output_dir << "/lidar_data_shifted.pcd" << std::endl;
|
|
|
sum << " " << output_dir << "/lidar_slice_start.pcd" << std::endl;
|
|
|
sum << " " << output_dir << "/lidar_slice_start.jpg" << std::endl;
|
|
|
sum << " " << output_dir << "/lidar_slice_end.pcd" << std::endl;
|
|
|
sum << " " << output_dir << "/lidar_slice_end.jpg" << std::endl;
|
|
|
sum << "==============================" << std::endl;
|
|
|
if (log_file.is_open()) log_file << gbk_to_utf8(sum.str());
|
|
|
}
|
|
|
log_file.close();
|
|
|
|
|
|
std::cout << "\n[截面] 完成。" << std::endl;
|
|
|
}
|
|
|
|
|
|
// ==================== 计数模式（count） ====================
|
|
|
// TCP收到 {"count":"start","box":{"l":行mm,"w":宽mm,"h":层高mm,"maxPerLayer":每层最大个数},"task_id":"xxx"} 开始扫描
|
|
|
// TCP收到 {"count":"stop"} 停止扫描并返回计数结果
|
|
|
// 高度计算: 物品高度 = Floor_Height - Y轴(mm)，参照Java: 地板值减去当前点的y轴值为高度
|
|
|
struct CountPoint
|
|
|
{
|
|
|
double x_mm;
|
|
|
double y_mm;
|
|
|
double z_val; // Z轴: 距离(mm)或时间(ms)，用于面积计算的第二维度
|
|
|
};
|
|
|
|
|
|
struct CountParams
|
|
|
{
|
|
|
double l = 0.0; // 行(长) mm
|
|
|
double w = 0.0; // 宽 mm
|
|
|
double h = 0.0; // 层高 mm
|
|
|
int maxPerLayer = 0; // 每层最大个数
|
|
|
std::string task_id;
|
|
|
};
|
|
|
|
|
|
static std::mutex count_mtx;
|
|
|
static bool count_running = false;
|
|
|
static bool count_stop = false;
|
|
|
static std::thread count_thread;
|
|
|
static std::vector<CountPoint> count_points;
|
|
|
static CountParams count_params;
|
|
|
static std::chrono::steady_clock::time_point count_start_tp;
|
|
|
|
|
|
// 后台采集线程：count start 后持续扫描，count stop 后退出
|
|
|
// start 后先丢弃SDK队列中积压的旧扫描数据(雷达一直转, start之前已缓存多圈数据)，
|
|
|
// 确保只统计 start 之后新扫描到的数据，否则积压的旧包会在图像开头z方向挤成一段密集点。
|
|
|
// z轴: 用本机时钟 elapsed_ms (相对清空积压后开始计时的时刻)。
|
|
|
static void count_collect_loop(pavo2::pavo2_driver* driver)
|
|
|
{
|
|
|
// 排空积压：把SDK缓存中 start 之前的数据包全部取出丢弃
|
|
|
pavo2::pavo_packet_info_scan_t flush_tmp;
|
|
|
int flushed = 0;
|
|
|
{
|
|
|
auto flush_begin = std::chrono::steady_clock::now();
|
|
|
while (true)
|
|
|
{
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(count_mtx);
|
|
|
if (count_stop) return; // 排空期间被stop，直接退出
|
|
|
}
|
|
|
if (driver->get_scans_available() <= 0) break;
|
|
|
if (!driver->get_scanned_data(flush_tmp, 0)) break; // 0ms = 立即返回，不阻塞
|
|
|
flushed++;
|
|
|
// 防御：雷达持续产生数据，最多排空500ms，避免卡死
|
|
|
if (std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
|
std::chrono::steady_clock::now() - flush_begin).count() > 500)
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
if (flushed > 0)
|
|
|
std::cout << "[count] 已丢弃积压扫描包 " << flushed << " 个" << std::endl;
|
|
|
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(count_mtx);
|
|
|
if (count_stop) return;
|
|
|
count_start_tp = std::chrono::steady_clock::now(); // 排空完成才开始计时
|
|
|
}
|
|
|
|
|
|
while (true)
|
|
|
{
|
|
|
pavo2::pavo_packet_info_scan_t scan_info;
|
|
|
if (driver->get_scanned_data(scan_info, 100)) // 100ms超时轮询，可及时响应stop
|
|
|
{
|
|
|
auto now = std::chrono::steady_clock::now();
|
|
|
auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - count_start_tp).count();
|
|
|
double z_val = (Speed_Z > 0)
|
|
|
? (Speed_Z * 10.0 * elapsed_ms / 1000.0) // cm/s × 10 = mm/s, × s = mm
|
|
|
: (double)elapsed_ms;
|
|
|
|
|
|
std::lock_guard<std::mutex> lk(count_mtx);
|
|
|
if (count_stop) break;
|
|
|
for (const auto& s : scan_info.vec_buff)
|
|
|
{
|
|
|
double angle_deg = s.angle / 100.0;
|
|
|
double angle_rad = angle_deg * 3.14159265 / 180.0;
|
|
|
double x_mm = s.distance * sin(angle_rad) * 2.0;
|
|
|
double y_mm = -s.distance * cos(angle_rad) * 2.0;
|
|
|
if (is_origin_point(x_mm, y_mm)) continue;
|
|
|
if (FILTER_ENABLE && !point_in_polygon(x_mm, y_mm, Filter_Polygon, Filter_Poly_N)) continue;
|
|
|
count_points.push_back({x_mm, y_mm, z_val});
|
|
|
}
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(count_mtx);
|
|
|
if (count_stop) break;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 简易JSON字段提取（仅用于内部命令解析）
|
|
|
static bool json_get_string(const std::string& json, const std::string& key, std::string& out)
|
|
|
{
|
|
|
std::string pat = "\"" + key + "\"";
|
|
|
size_t pos = json.find(pat);
|
|
|
if (pos == std::string::npos) return false;
|
|
|
size_t colon = json.find(':', pos + pat.size());
|
|
|
if (colon == std::string::npos) return false;
|
|
|
size_t p = colon + 1;
|
|
|
while (p < json.size() && (json[p] == ' ' || json[p] == '\t')) p++;
|
|
|
if (p < json.size() && json[p] == '"')
|
|
|
{
|
|
|
size_t q = json.find('"', p + 1);
|
|
|
if (q == std::string::npos) return false;
|
|
|
out = json.substr(p + 1, q - p - 1);
|
|
|
return true;
|
|
|
}
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
static bool json_get_number(const std::string& json, const std::string& key, double& out)
|
|
|
{
|
|
|
std::string pat = "\"" + key + "\"";
|
|
|
size_t pos = json.find(pat);
|
|
|
if (pos == std::string::npos) return false;
|
|
|
size_t colon = json.find(':', pos + pat.size());
|
|
|
if (colon == std::string::npos) return false;
|
|
|
size_t p = colon + 1;
|
|
|
while (p < json.size() && (json[p] == ' ' || json[p] == '\t')) p++;
|
|
|
size_t end = p;
|
|
|
while (end < json.size() && (std::isdigit((unsigned char)json[end]) || json[end] == '.' ||
|
|
|
json[end] == '-' || json[end] == '+' || json[end] == 'e' || json[end] == 'E')) end++;
|
|
|
if (end == p) return false;
|
|
|
try { out = std::stod(json.substr(p, end - p)); return true; }
|
|
|
catch (...) { return false; }
|
|
|
}
|
|
|
|
|
|
// 参照Java drawPointsAndCalculateArea：点集 → 图像(8x8点)
|
|
|
// → 统计非白像素 → 换算实际面积
|
|
|
// 宽: 由 Filter_Polygon 多边形 X 最大最小值构成；高: 由 z 最大值构成(Z为运动方向,从0开始)
|
|
|
// 绘制平面取 (x_mm, z_val)：X为宽度方向，Z为运动方向
|
|
|
// 返回白底图像(0=白,255=黑点)，便于保存为JPG；area_out返回换算面积；out_w/out_h返回实际图像尺寸
|
|
|
static std::vector<uint8_t> points_to_image(const std::vector<CountPoint>& points,
|
|
|
double* area_out = nullptr,
|
|
|
int* out_w = nullptr,
|
|
|
int* out_h = nullptr)
|
|
|
{
|
|
|
const int MIN_DIM = 8; // 最小尺寸保护
|
|
|
const int MAX_DIM = 20000; // 单边最大尺寸保护(防止z过大导致内存爆炸)
|
|
|
if (area_out) *area_out = 0.0;
|
|
|
if (out_w) *out_w = 0;
|
|
|
if (out_h) *out_h = 0;
|
|
|
if (points.empty()) return std::vector<uint8_t>((size_t)MIN_DIM * MIN_DIM, 0);
|
|
|
|
|
|
// 宽: Filter_Polygon 多边形 X 最大最小值直接构成 (1mm=1px)
|
|
|
double polyXMin = 0.0, polyXMax = 0.0;
|
|
|
if (Filter_Poly_N > 0)
|
|
|
{
|
|
|
polyXMin = Filter_Polygon[0].first;
|
|
|
polyXMax = Filter_Polygon[0].first;
|
|
|
for (int i = 1; i < Filter_Poly_N; i++)
|
|
|
{
|
|
|
polyXMin = std::min(polyXMin, Filter_Polygon[i].first);
|
|
|
polyXMax = std::max(polyXMax, Filter_Polygon[i].first);
|
|
|
}
|
|
|
}
|
|
|
double spanX = polyXMax - polyXMin;
|
|
|
if (spanX <= 0.0) spanX = 1.0;
|
|
|
|
|
|
// 高: z 最大值直接构成 (Z=运动方向, 从0开始递增, 1单位=1px)
|
|
|
double maxZ = 0.0;
|
|
|
for (const auto& p : points) maxZ = std::max(maxZ, p.z_val);
|
|
|
if (maxZ <= 0.0) maxZ = 1.0;
|
|
|
|
|
|
int imageWidth = (int)std::lround(spanX);
|
|
|
int imageHeight = (int)std::lround(maxZ);
|
|
|
if (imageWidth < MIN_DIM) imageWidth = MIN_DIM;
|
|
|
if (imageHeight < MIN_DIM) imageHeight = MIN_DIM;
|
|
|
if (imageWidth > MAX_DIM) imageWidth = MAX_DIM;
|
|
|
if (imageHeight > MAX_DIM) imageHeight = MAX_DIM;
|
|
|
if (out_w) *out_w = imageWidth;
|
|
|
if (out_h) *out_h = imageHeight;
|
|
|
std::vector<uint8_t> image((size_t)imageWidth * imageHeight, 0); // 0=白底
|
|
|
|
|
|
// X坐标相对多边形左边界偏移到0；Z从0开始
|
|
|
double scaleX = (double)imageWidth / spanX; // ≈1: 宽直接由多边形X跨度构成
|
|
|
double scaleY = (double)imageHeight / maxZ; // ≈1: 高直接由z最大值构成
|
|
|
double xOffset = -polyXMin;
|
|
|
|
|
|
// 优化：Step1 先按1x1网格标记点位置（重复点只写一次），
|
|
|
// Step2 再对标记位置膨胀 Point_Dilate×Point_Dilate（与逐点画 Point_Dilate×Point_Dilate 数学等价，写次数大幅减少）
|
|
|
std::vector<uint32_t> marks;
|
|
|
marks.reserve(points.size() / 2);
|
|
|
for (const auto& p : points)
|
|
|
{
|
|
|
int px = (int)((p.x_mm + xOffset) * scaleX);
|
|
|
int py = (int)(p.z_val * scaleY);
|
|
|
if (px < 0 || px >= imageWidth || py < 0 || py >= imageHeight) continue;
|
|
|
size_t idx = (size_t)py * imageWidth + px;
|
|
|
if (image[idx] == 0) // 未标记过才记录
|
|
|
{
|
|
|
image[idx] = 1;
|
|
|
marks.push_back((uint32_t)idx);
|
|
|
}
|
|
|
}
|
|
|
for (uint32_t idx : marks) // 膨胀 Point_Dilate×Point_Dilate
|
|
|
{
|
|
|
int px = (int)(idx % imageWidth);
|
|
|
int py = (int)(idx / imageWidth);
|
|
|
for (int dy = 0; dy < Point_Dilate; dy++)
|
|
|
for (int dx = 0; dx < Point_Dilate; dx++)
|
|
|
{
|
|
|
int ix = px + dx, iy = py + dy;
|
|
|
if (ix < imageWidth && iy < imageHeight)
|
|
|
image[(size_t)iy * imageWidth + ix] = 255; // 255=黑点
|
|
|
}
|
|
|
}
|
|
|
int totalPixels = 0;
|
|
|
for (size_t i = 0; i < image.size(); i++)
|
|
|
if (image[i] != 0) totalPixels++;
|
|
|
|
|
|
double pixelToAreaRatio = spanX * maxZ / (double)(imageWidth * imageHeight);
|
|
|
if (area_out) *area_out = totalPixels * pixelToAreaRatio;
|
|
|
return image;
|
|
|
}
|
|
|
|
|
|
static double draw_points_calc_area(const std::vector<CountPoint>& points)
|
|
|
{
|
|
|
double area = 0.0;
|
|
|
points_to_image(points, &area);
|
|
|
return area;
|
|
|
}
|
|
|
|
|
|
// ==================== z轴空隙插值 ====================
|
|
|
// 雷达某段时间无数据(断流/瞬间丢包)时, z方向会出现空白带。
|
|
|
// 原理: 同一scan包(一圈)的所有点 z 相同, 正常圈间隔近似恒定(取相邻圈z差的中位数);
|
|
|
// 若某两个相邻圈的 z 差远大于正常间隔 → 判定为空隙, 用前/后圈数据补出虚拟圈。
|
|
|
// Z_Interpolate_Mode:
|
|
|
// none = 不插值
|
|
|
// CopyTranslation = 空隙内按正常间隔分层, 复制"前一圈"的点平移填充
|
|
|
// AngleBucketInterpolation = 空隙内以前/后两圈做"角度桶"插值生成虚拟圈(平滑过渡)
|
|
|
// 返回新增点数
|
|
|
static int z_gap_interpolate(std::vector<CountPoint>& points)
|
|
|
{
|
|
|
if (points.empty() || Z_Interpolate_Mode == "none")
|
|
|
return 0;
|
|
|
|
|
|
// 正常相邻两条(圈)的 z 间隔: 速度模式 = 每圈行进距离(mm); 时间模式 = 每圈耗时(ms)
|
|
|
double normal_gap = (Speed_Z > 0)
|
|
|
? (Speed_Z * 10.0 / Lidar_Hz) // cm/s ×10 = mm/s, ÷Hz = mm/圈
|
|
|
: (1000.0 / Lidar_Hz); // ms/圈
|
|
|
if (normal_gap <= 0) return 0;
|
|
|
|
|
|
// 空隙判定: 本条与上一条 z 差 > 2×正常间隔 视为断流空隙
|
|
|
const double gap_threshold = 2.0 * normal_gap;
|
|
|
// 条件1: 前后两条 Y 平均值差超过30mm 不插(可能跨层/换物体)
|
|
|
const double max_y_mean_diff = 30.0;
|
|
|
// 一个空隙最多补的行数
|
|
|
const int MAX_FILL_LEVELS = 10;
|
|
|
|
|
|
// 1. 按 z 升序排序
|
|
|
std::sort(points.begin(), points.end(),
|
|
|
[](const CountPoint& a, const CountPoint& b) { return a.z_val < b.z_val; });
|
|
|
|
|
|
// 2. 聚类成"圈"(同一scan包的点 z 完全相同, 用区间 [begin,end) 索引 points)
|
|
|
struct Circle { double z; size_t begin; size_t end; };
|
|
|
std::vector<Circle> circles;
|
|
|
{
|
|
|
size_t i = 0;
|
|
|
while (i < points.size())
|
|
|
{
|
|
|
size_t j = i + 1;
|
|
|
while (j < points.size() && points[j].z_val - points[i].z_val < 1e-6) ++j;
|
|
|
circles.push_back(Circle());
|
|
|
circles.back().z = points[i].z_val;
|
|
|
circles.back().begin = i;
|
|
|
circles.back().end = j;
|
|
|
i = j;
|
|
|
}
|
|
|
}
|
|
|
if (circles.size() < 2)
|
|
|
return 0;
|
|
|
|
|
|
auto circle_mean_y = [&](const Circle& c) -> double
|
|
|
{
|
|
|
double s = 0;
|
|
|
for (size_t p = c.begin; p < c.end; ++p) s += points[p].y_mm;
|
|
|
return s / (double)(c.end - c.begin);
|
|
|
};
|
|
|
|
|
|
int added = 0;
|
|
|
// 3. 逐空隙补行
|
|
|
for (size_t i = 1; i < circles.size(); ++i)
|
|
|
{
|
|
|
double gap = circles[i].z - circles[i - 1].z;
|
|
|
if (gap <= gap_threshold) // 未超过2×正常间隔, 不算空隙
|
|
|
continue;
|
|
|
|
|
|
const Circle& cA = circles[i - 1]; // 上一条
|
|
|
const Circle& cB = circles[i]; // 本条
|
|
|
|
|
|
// 条件1: 前后两条的Y平均值几乎相当(差≤30mm)才插值
|
|
|
if (std::fabs(circle_mean_y(cA) - circle_mean_y(cB)) > max_y_mean_diff)
|
|
|
continue;
|
|
|
|
|
|
// 条件2: 生成行的 X 范围 = 这两条的 X 最大和最小
|
|
|
double x_lo = points[cA.begin].x_mm, x_hi = points[cA.begin].x_mm;
|
|
|
for (size_t p = cA.begin; p < cA.end; ++p)
|
|
|
{
|
|
|
if (points[p].x_mm < x_lo) x_lo = points[p].x_mm;
|
|
|
if (points[p].x_mm > x_hi) x_hi = points[p].x_mm;
|
|
|
}
|
|
|
for (size_t p = cB.begin; p < cB.end; ++p)
|
|
|
{
|
|
|
if (points[p].x_mm < x_lo) x_lo = points[p].x_mm;
|
|
|
if (points[p].x_mm > x_hi) x_hi = points[p].x_mm;
|
|
|
}
|
|
|
|
|
|
// 空隙内补行数: 按正常间隔容纳行数-1(空隙两端已有), 最多10行
|
|
|
int n = (int)(gap / normal_gap + 0.5) - 1;
|
|
|
if (n < 1) continue;
|
|
|
if (n > MAX_FILL_LEVELS) n = MAX_FILL_LEVELS;
|
|
|
|
|
|
if (Z_Interpolate_Mode == "CopyTranslation")
|
|
|
{
|
|
|
// 复制"上一条"的点, z 均匀平移到空隙内的各补行
|
|
|
for (int k = 1; k <= n; ++k)
|
|
|
{
|
|
|
double zk = cA.z + gap * (double)k / (n + 1); // 均匀分布
|
|
|
for (size_t p = cA.begin; p < cA.end; ++p)
|
|
|
{
|
|
|
const CountPoint& src = points[p];
|
|
|
if (src.x_mm < x_lo || src.x_mm > x_hi) continue; // X范围约束
|
|
|
CountPoint np = src;
|
|
|
np.z_val = zk;
|
|
|
points.push_back(np);
|
|
|
++added;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
else if (Z_Interpolate_Mode == "AngleBucketInterpolation")
|
|
|
{
|
|
|
// 角度桶插值: 每桶用前A/后B平均点线性插值(平滑过渡), 同样受Y平均/X范围条件约束
|
|
|
struct Buck { double sx, sy; int cnt; Buck() : sx(0), sy(0), cnt(0) {} };
|
|
|
std::vector<Buck> bA(360), bB(360);
|
|
|
auto fill = [&](const Circle& c, std::vector<Buck>& out)
|
|
|
{
|
|
|
for (size_t p = c.begin; p < c.end; ++p)
|
|
|
{
|
|
|
double deg = std::atan2(points[p].x_mm, -points[p].y_mm) * 57.29577951308232;
|
|
|
if (deg < 0) deg += 360.0;
|
|
|
int b = (int)(deg + 0.5);
|
|
|
if (b >= 360) b = 0;
|
|
|
out[b].sx += points[p].x_mm;
|
|
|
out[b].sy += points[p].y_mm;
|
|
|
out[b].cnt++;
|
|
|
}
|
|
|
};
|
|
|
fill(cA, bA);
|
|
|
fill(cB, bB);
|
|
|
for (int k = 1; k <= n; ++k)
|
|
|
{
|
|
|
double zk = cA.z + gap * (double)k / (n + 1); // 均匀分布
|
|
|
double t = (zk - cA.z) / gap; // 0~1
|
|
|
for (int b = 0; b < 360; ++b)
|
|
|
{
|
|
|
if (bA[b].cnt <= 0 || bB[b].cnt <= 0) continue;
|
|
|
double ax = bA[b].sx / bA[b].cnt, ay = bA[b].sy / bA[b].cnt;
|
|
|
double bx = bB[b].sx / bB[b].cnt, by = bB[b].sy / bB[b].cnt;
|
|
|
double nx = ax + (bx - ax) * t;
|
|
|
double ny = ay + (by - ay) * t;
|
|
|
if (nx < x_lo || nx > x_hi) continue; // X范围约束
|
|
|
CountPoint np;
|
|
|
np.x_mm = nx;
|
|
|
np.y_mm = ny;
|
|
|
np.z_val = zk;
|
|
|
points.push_back(np);
|
|
|
++added;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
if (added > 0)
|
|
|
std::cout << "[count] z空隙插值: 模式=" << Z_Interpolate_Mode
|
|
|
<< ", 正常间隔=" << normal_gap
|
|
|
<< ", 判定阈值=2×间隔=" << gap_threshold
|
|
|
<< ", 新增" << added << "点" << std::endl;
|
|
|
return added;
|
|
|
}
|
|
|
|
|
|
// 参照Java getLongitudinalType：按层统计总数
|
|
|
// 高度 = Floor_Height - Y轴; level = round(高度/层高);
|
|
|
// 冗余: 第1层50mm, 每高一层+20mm; 从最高层(5)往下找有效层, 该层面积/(行×宽) 得该层个数
|
|
|
// detail: 可选输出每层点数/面积详情（供日志使用）
|
|
|
// top_layer_img/level/area: 可选输出被计数层(最高有效层)的图像/层号/面积
|
|
|
static int calc_item_count(const std::vector<CountPoint>& points, const CountParams& box,
|
|
|
std::string* detail = nullptr,
|
|
|
std::vector<uint8_t>* top_layer_img = nullptr,
|
|
|
int* top_layer_img_w = nullptr,
|
|
|
int* top_layer_img_h = nullptr,
|
|
|
int* top_layer_level = nullptr,
|
|
|
double* top_layer_area = nullptr)
|
|
|
{
|
|
|
if (points.empty() || box.l <= 0 || box.w <= 0 || box.h <= 0 || box.maxPerLayer <= 0)
|
|
|
return 0;
|
|
|
|
|
|
std::unordered_map<int, std::vector<CountPoint>> layer_map;
|
|
|
for (const auto& p : points)
|
|
|
{
|
|
|
if (FILTER_ENABLE && !point_in_polygon(p.x_mm, p.y_mm, Filter_Polygon, Filter_Poly_N))
|
|
|
continue;
|
|
|
double currentHeight = Floor_Height - p.y_mm; // 高度 = 地板值 - Y轴
|
|
|
if (currentHeight < 0) continue;
|
|
|
int level = (int)std::round(currentHeight / box.h);
|
|
|
double tolerance = Count_Base_Tolerance + level * Count_Additional_Tolerance_Per_Level;
|
|
|
if (std::abs(currentHeight - level * box.h) <= tolerance)
|
|
|
layer_map[level].push_back(p);
|
|
|
}
|
|
|
|
|
|
// 每层面积/图像只算一次，详情与结果共用，避免重复构建图像
|
|
|
std::ostringstream oss;
|
|
|
std::unordered_map<int, double> layer_area;
|
|
|
std::unordered_map<int, std::vector<uint8_t>> layer_img;
|
|
|
std::unordered_map<int, std::pair<int, int>> layer_img_size;
|
|
|
for (int h = 5; h > 0; h--)
|
|
|
{
|
|
|
auto it = layer_map.find(h);
|
|
|
if (it == layer_map.end()) continue;
|
|
|
oss << " 层" << h << ": 点数=" << it->second.size();
|
|
|
// 高度范围: 该层算法对应的理论高度区间(即生成该层count_layer截取的高度范围) - 每层都显示
|
|
|
double tol = Count_Base_Tolerance + h * Count_Additional_Tolerance_Per_Level;
|
|
|
double h_lo = h * box.h - tol;
|
|
|
double h_hi = h * box.h + tol;
|
|
|
oss << ", 高度范围（" << std::setprecision(0) << h_lo << "," << h_hi << "）";
|
|
|
if (it->second.size() > 1000)
|
|
|
{
|
|
|
double area = 0.0;
|
|
|
int img_w = 0, img_h = 0;
|
|
|
std::vector<uint8_t> img = points_to_image(it->second, &area, &img_w, &img_h);
|
|
|
layer_area[h] = area;
|
|
|
layer_img[h] = std::move(img);
|
|
|
layer_img_size[h] = std::make_pair(img_w, img_h);
|
|
|
double item_cnt = area / (box.l * box.w); // 未取整的个数(面积/单件面积)
|
|
|
oss << ", 面积=" << std::fixed << std::setprecision(1) << area << "mm^2"
|
|
|
<< ", 个数" << std::setprecision(2) << item_cnt << "个";
|
|
|
}
|
|
|
oss << "\n";
|
|
|
}
|
|
|
|
|
|
int layersCount = box.maxPerLayer; // 每层最大个数
|
|
|
int result = 0;
|
|
|
int top_lv = 0;
|
|
|
double top_area = 0.0;
|
|
|
for (int h = 5; h > 0; h--) // 最多5层，从顶层往下找
|
|
|
{
|
|
|
auto it = layer_map.find(h);
|
|
|
if (it != layer_map.end() && it->second.size() > 1000)
|
|
|
{
|
|
|
double area = layer_area[h];
|
|
|
if (area > 0)
|
|
|
{
|
|
|
int i = (int)std::min(std::round(area / (box.l * box.w)), (double)layersCount);
|
|
|
if (i > 0)
|
|
|
{
|
|
|
result = layersCount * (h - 1) + i;
|
|
|
top_lv = h;
|
|
|
top_area = area;
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
if (detail) *detail = oss.str();
|
|
|
// 输出被计数层(最高有效层)的图像/尺寸/层号/面积
|
|
|
if (top_layer_img && top_lv > 0)
|
|
|
{
|
|
|
auto it = layer_img.find(top_lv);
|
|
|
if (it != layer_img.end()) *top_layer_img = it->second;
|
|
|
auto sit = layer_img_size.find(top_lv);
|
|
|
if (sit != layer_img_size.end())
|
|
|
{
|
|
|
if (top_layer_img_w) *top_layer_img_w = sit->second.first;
|
|
|
if (top_layer_img_h) *top_layer_img_h = sit->second.second;
|
|
|
}
|
|
|
}
|
|
|
if (top_layer_level) *top_layer_level = top_lv;
|
|
|
if (top_layer_area) *top_layer_area = top_area;
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
// ==================== TCP服务端模式 ====================
|
|
|
// 收到 {"check":1} 触发一次检测（demo_collect_multi）
|
|
|
void handle_tcp_client(SOCKET client, pavo2::pavo2_driver* driver)
|
|
|
{
|
|
|
static std::mutex check_mutex; // 串行化检测，避免并发
|
|
|
|
|
|
char buf[1024];
|
|
|
std::string recv_buf;
|
|
|
while (true)
|
|
|
{
|
|
|
int n = recv(client, buf, sizeof(buf) - 1, 0);
|
|
|
if (n <= 0) break; // 连接关闭或出错
|
|
|
buf[n] = '\0';
|
|
|
recv_buf.append(buf, n);
|
|
|
|
|
|
// 按行处理消息
|
|
|
size_t pos;
|
|
|
while ((pos = recv_buf.find('\n')) != std::string::npos)
|
|
|
{
|
|
|
std::string line = recv_buf.substr(0, pos);
|
|
|
recv_buf.erase(0, pos + 1);
|
|
|
|
|
|
// 去空白后匹配命令: {"check":1}=触发检测, {"config":1}=重读配置
|
|
|
std::string cleaned;
|
|
|
for (char c : line)
|
|
|
if (c != ' ' && c != '\t' && c != '\r' && c != '\n') cleaned += c;
|
|
|
|
|
|
if (cleaned == "{\"config\":1}")
|
|
|
{
|
|
|
std::cout << "[TCP] 收到配置重载命令，重新读取 config/config ..." << std::endl;
|
|
|
load_config();
|
|
|
std::cout << "[TCP] 配置已重载: 采集=" << Collect_Duration_S << "s, 速度=" << Speed_Z
|
|
|
<< "cm/s, Z区间=" << Z_RANGE_MM << "mm, 帧率=" << Lidar_Hz
|
|
|
<< "Hz, 矩形宽=" << Rect_Width_PX << "px, 模式=" << Run_Mode
|
|
|
<< ", 端口=" << Tcp_Port << std::endl;
|
|
|
const char* ack = "{\"code\":200,\"message\":\"config reloaded\"}\n";
|
|
|
send(client, ack, (int)std::strlen(ack), 0);
|
|
|
}
|
|
|
else if (cleaned == "{\"check\":1}")
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(check_mutex);
|
|
|
std::cout << "[TCP] 收到检测命令: " << line << "，开始检测..." << std::endl;
|
|
|
const char* ack = "{\"code\":200,\"message\":\"check received, detecting...\"}\n";
|
|
|
send(client, ack, (int)std::strlen(ack), 0);
|
|
|
|
|
|
DetectResult res;
|
|
|
demo_collect_multi(*driver, &res);
|
|
|
|
|
|
// 返回检测完成 + start/end 白色占比 + Y最大/中位数
|
|
|
std::ostringstream resp;
|
|
|
resp << "{\"code\":200,\"message\":\"detect done\"";
|
|
|
if (res.valid) {
|
|
|
resp << ",\"start\":{\"fill_pct\":"
|
|
|
<< std::fixed << std::setprecision(2) << res.start_fill_pct
|
|
|
<< ",\"y_max\":" << std::setprecision(1) << res.start_y_max
|
|
|
<< ",\"y_median\":" << res.start_y_med << "}";
|
|
|
resp << ",\"end\":{\"fill_pct\":"
|
|
|
<< std::fixed << std::setprecision(2) << res.end_fill_pct
|
|
|
<< ",\"y_max\":" << std::setprecision(1) << res.end_y_max
|
|
|
<< ",\"y_median\":" << res.end_y_med << "}";
|
|
|
}
|
|
|
resp << "}\n";
|
|
|
std::string resp_str = resp.str();
|
|
|
send(client, resp_str.c_str(), (int)resp_str.size(), 0);
|
|
|
std::cout << "[TCP] 检测完成，已返回结果。" << std::endl;
|
|
|
}
|
|
|
else if (cleaned.find("\"count\"") != std::string::npos)
|
|
|
{
|
|
|
// count 计数命令: {"count":"start","box":{"l":行mm,"w":宽mm,"h":层高mm,"maxPerLayer":每层最大个数},"task_id":"xxx"}
|
|
|
// {"count":"stop"}
|
|
|
std::string count_cmd;
|
|
|
if (!json_get_string(cleaned, "count", count_cmd))
|
|
|
{
|
|
|
const char* ack = "{\"code\":400,\"message\":\"count parse error\"}\n";
|
|
|
send(client, ack, (int)std::strlen(ack), 0);
|
|
|
continue;
|
|
|
}
|
|
|
|
|
|
if (count_cmd == "start")
|
|
|
{
|
|
|
CountParams params;
|
|
|
json_get_string(cleaned, "task_id", params.task_id);
|
|
|
json_get_number(cleaned, "l", params.l);
|
|
|
json_get_number(cleaned, "w", params.w);
|
|
|
json_get_number(cleaned, "h", params.h);
|
|
|
double d = 0.0;
|
|
|
if (json_get_number(cleaned, "maxPerLayer", d) || json_get_number(cleaned, "max", d))
|
|
|
params.maxPerLayer = (int)d;
|
|
|
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(count_mtx);
|
|
|
if (count_running)
|
|
|
{
|
|
|
const char* ack = "{\"code\":400,\"message\":\"count already running\"}\n";
|
|
|
send(client, ack, (int)std::strlen(ack), 0);
|
|
|
continue;
|
|
|
}
|
|
|
count_params = params;
|
|
|
count_points.clear();
|
|
|
count_stop = false;
|
|
|
count_running = true;
|
|
|
count_thread = std::thread(count_collect_loop, driver);
|
|
|
}
|
|
|
|
|
|
std::cout << "[TCP] 收到count:start, task_id=" << params.task_id
|
|
|
<< ", box(l=" << params.l << ",w=" << params.w
|
|
|
<< ",h=" << params.h << ",maxPerLayer=" << params.maxPerLayer
|
|
|
<< ")，开始扫描..." << std::endl;
|
|
|
std::ostringstream resp;
|
|
|
resp << "{\"code\":200,\"message\":\"count start\"";
|
|
|
if (!params.task_id.empty())
|
|
|
resp << ",\"task_id\":\"" << params.task_id << "\"";
|
|
|
resp << "}\n";
|
|
|
std::string resp_str = resp.str();
|
|
|
send(client, resp_str.c_str(), (int)resp_str.size(), 0);
|
|
|
}
|
|
|
else if (count_cmd == "stop")
|
|
|
{
|
|
|
std::string task_id;
|
|
|
json_get_string(cleaned, "task_id", task_id);
|
|
|
|
|
|
// 只有真正 start 过的 stop 才有效，否则返回 0
|
|
|
bool was_running = false;
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(count_mtx);
|
|
|
if (count_running)
|
|
|
{
|
|
|
count_stop = true;
|
|
|
count_running = false;
|
|
|
was_running = true;
|
|
|
}
|
|
|
}
|
|
|
if (count_thread.joinable()) count_thread.join();
|
|
|
|
|
|
int total = 0;
|
|
|
size_t pt_cnt = 0;
|
|
|
std::string layer_detail;
|
|
|
CountParams params_for_log; // 本次任务的box参数（日志用）
|
|
|
std::vector<CountPoint> saved_points;
|
|
|
std::vector<uint8_t> top_img; // 被计数层的图像
|
|
|
int top_lv = 0;
|
|
|
int top_img_w = 0, top_img_h = 0;
|
|
|
double top_area = 0.0;
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(count_mtx);
|
|
|
saved_points = count_points;
|
|
|
z_gap_interpolate(saved_points); // z轴空隙插值(none时不处理)
|
|
|
pt_cnt = saved_points.size();
|
|
|
params_for_log = count_params;
|
|
|
if (was_running && pt_cnt > 0 && count_params.l > 0 &&
|
|
|
count_params.w > 0 && count_params.h > 0 && count_params.maxPerLayer > 0)
|
|
|
total = calc_item_count(saved_points, count_params, &layer_detail,
|
|
|
&top_img, &top_img_w, &top_img_h,
|
|
|
&top_lv, &top_area);
|
|
|
// 清空遗留数据/参数，避免未start的stop复用旧数据重复计数
|
|
|
count_points.clear();
|
|
|
count_params = CountParams();
|
|
|
}
|
|
|
|
|
|
std::cout << "[TCP] 收到count:stop, task_id=" << task_id
|
|
|
<< ", 采集点数=" << pt_cnt << ", 计数结果=" << total << std::endl;
|
|
|
|
|
|
// 先返回结果，PCD/图片/日志放到后台线程保存，不阻塞响应
|
|
|
std::ostringstream resp;
|
|
|
resp << "{\"count\":" << total;
|
|
|
if (!task_id.empty())
|
|
|
resp << ",\"task_id\":\"" << task_id << "\"";
|
|
|
resp << "}\n";
|
|
|
std::string resp_str = resp.str();
|
|
|
send(client, resp_str.c_str(), (int)resp_str.size(), 0);
|
|
|
|
|
|
if (was_running && pt_cnt > 0)
|
|
|
{
|
|
|
// C++11 兼容：先 move 到局部变量，再按值捕获
|
|
|
std::vector<CountPoint> bg_points = std::move(saved_points);
|
|
|
std::vector<uint8_t> bg_img = std::move(top_img);
|
|
|
std::thread([bg_points, bg_img, params_for_log, layer_detail, task_id,
|
|
|
total, pt_cnt, top_lv, top_area,
|
|
|
top_img_w, top_img_h]() {
|
|
|
// 目录: data/日期/时间
|
|
|
time_t rawtime = std::time(nullptr);
|
|
|
struct tm* timeinfo = std::localtime(&rawtime);
|
|
|
char dir_buf[32];
|
|
|
std::strftime(dir_buf, sizeof(dir_buf), "data/%Y%m%d/%H%M%S", timeinfo);
|
|
|
std::string out_dir(dir_buf);
|
|
|
mkdir("data");
|
|
|
mkdir(out_dir.substr(0, out_dir.rfind('/')).c_str());
|
|
|
_mkdir(out_dir.c_str());
|
|
|
|
|
|
// PCD：y 换成高度(Floor_Height - y_mm)
|
|
|
std::vector<double> xs, ys, zs;
|
|
|
xs.reserve(pt_cnt); ys.reserve(pt_cnt); zs.reserve(pt_cnt);
|
|
|
for (const auto& p : bg_points)
|
|
|
{
|
|
|
xs.push_back(p.x_mm);
|
|
|
ys.push_back(Floor_Height - p.y_mm);
|
|
|
zs.push_back(p.z_val);
|
|
|
}
|
|
|
save_pcd_ascii(out_dir + "/count_data.pcd", xs, ys, zs);
|
|
|
std::cout << "[count] 点云已保存: " << out_dir << "/count_data.pcd ("
|
|
|
<< pt_cnt << " 点, Y=高度)" << std::endl;
|
|
|
|
|
|
// 图片1：全点X-Z平面图(白底黑点, 尺寸按实际X/Z跨度比例)
|
|
|
double all_area = 0.0;
|
|
|
int all_w = 0, all_h = 0;
|
|
|
std::vector<uint8_t> all_img = points_to_image(bg_points, &all_area, &all_w, &all_h);
|
|
|
if (save_jpg_gray(out_dir + "/count_points.jpg", all_w, all_h, all_img))
|
|
|
std::cout << "[count] 点云图已保存: " << out_dir << "/count_points.jpg ("
|
|
|
<< all_w << "x" << all_h << ", X-Z平面, 面积≈"
|
|
|
<< std::fixed << std::setprecision(1)
|
|
|
<< all_area << "mm^2)" << std::endl;
|
|
|
|
|
|
// 图片2：被计数层(最高有效层)图像（Java drawPointsAndCalculateArea语义）
|
|
|
if (top_lv > 0 && !bg_img.empty() && top_img_w > 0 && top_img_h > 0)
|
|
|
{
|
|
|
if (save_jpg_gray(out_dir + "/count_layer.jpg", top_img_w, top_img_h, bg_img))
|
|
|
std::cout << "[count] 有效层图已保存: " << out_dir << "/count_layer.jpg ("
|
|
|
<< top_img_w << "x" << top_img_h << ", 层" << top_lv
|
|
|
<< ", 面积≈" << std::fixed
|
|
|
<< std::setprecision(1) << top_area << "mm^2)" << std::endl;
|
|
|
}
|
|
|
|
|
|
// 日志
|
|
|
if (Log_Flag)
|
|
|
{
|
|
|
std::ofstream lf(out_dir + "/log.txt");
|
|
|
if (lf.is_open())
|
|
|
{
|
|
|
std::ostringstream sum;
|
|
|
sum << "==============================\n";
|
|
|
sum << "[count] task_id: " << task_id << "\n";
|
|
|
sum << "[count] box: l=" << params_for_log.l
|
|
|
<< ", w=" << params_for_log.w
|
|
|
<< ", h=" << params_for_log.h
|
|
|
<< ", maxPerLayer=" << params_for_log.maxPerLayer << "\n";
|
|
|
sum << "[count] Floor_Height: " << Floor_Height << " mm\n";
|
|
|
sum << "[count] 采集点数: " << pt_cnt << "\n";
|
|
|
sum << "[count] 计数层: 层" << top_lv
|
|
|
<< " (面积=" << std::fixed << std::setprecision(1)
|
|
|
<< top_area << "mm^2)\n";
|
|
|
sum << "[count] 分层情况:\n" << layer_detail;
|
|
|
sum << "[count] 计数结果: " << total << "\n";
|
|
|
sum << "[count] 输出文件:\n";
|
|
|
sum << " " << out_dir << "/count_data.pcd\n";
|
|
|
sum << " " << out_dir << "/count_points.jpg (" << all_w << "x" << all_h << ")\n";
|
|
|
if (top_lv > 0 && !bg_img.empty() && top_img_w > 0 && top_img_h > 0)
|
|
|
sum << " " << out_dir << "/count_layer.jpg ("
|
|
|
<< top_img_w << "x" << top_img_h << ")\n";
|
|
|
sum << "==============================\n";
|
|
|
lf << gbk_to_utf8(sum.str());
|
|
|
lf.close();
|
|
|
}
|
|
|
}
|
|
|
std::cout << "[count] 输出目录: " << out_dir << std::endl;
|
|
|
}).detach();
|
|
|
}
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
const char* ack = "{\"code\":400,\"message\":\"unknown count command\"}\n";
|
|
|
send(client, ack, (int)std::strlen(ack), 0);
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
closesocket(client);
|
|
|
}
|
|
|
|
|
|
void run_tcp_server(pavo2::pavo2_driver& driver)
|
|
|
{
|
|
|
std::cout << "\n========== TCP服务端模式 ==========" << std::endl;
|
|
|
std::cout << "[TCP] 监听端口: " << Tcp_Port << std::endl;
|
|
|
|
|
|
SOCKET listen_sock = socket(AF_INET, SOCK_STREAM, 0);
|
|
|
if (listen_sock == INVALID_SOCKET)
|
|
|
{
|
|
|
std::cerr << "[错误] 创建监听socket失败!" << std::endl;
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
sockaddr_in addr;
|
|
|
addr.sin_family = AF_INET;
|
|
|
addr.sin_addr.s_addr = INADDR_ANY;
|
|
|
addr.sin_port = htons(Tcp_Port);
|
|
|
if (bind(listen_sock, (sockaddr*)&addr, sizeof(addr)) == SOCKET_ERROR)
|
|
|
{
|
|
|
std::cerr << "[错误] 绑定端口 " << Tcp_Port << " 失败!" << std::endl;
|
|
|
closesocket(listen_sock);
|
|
|
return;
|
|
|
}
|
|
|
if (listen(listen_sock, 5) == SOCKET_ERROR)
|
|
|
{
|
|
|
std::cerr << "[错误] 监听失败!" << std::endl;
|
|
|
closesocket(listen_sock);
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
std::cout << "[TCP] 初始化完成，等待检测命令 {\"check\":1} ..." << std::endl;
|
|
|
while (true)
|
|
|
{
|
|
|
SOCKET client = accept(listen_sock, nullptr, nullptr);
|
|
|
if (client == INVALID_SOCKET)
|
|
|
{
|
|
|
std::cout << "[TCP] accept失败，重试..." << std::endl;
|
|
|
continue;
|
|
|
}
|
|
|
std::thread(handle_tcp_client, client, &driver).detach();
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// ==================== 主函数 ====================
|
|
|
int main()
|
|
|
{
|
|
|
#ifdef _WIN32
|
|
|
//SetConsoleOutputCP(CP_UTF8); 20260714临时去掉，应对乱码
|
|
|
|
|
|
WSADATA wsa_data;
|
|
|
if (WSAStartup(MAKEWORD(2, 2), &wsa_data) != 0)
|
|
|
{
|
|
|
std::cerr << "[错误] WSAStartup 初始化失败！" << std::endl;
|
|
|
return -1;
|
|
|
}
|
|
|
#endif
|
|
|
load_config(); //加载配置文件0713增加
|
|
|
|
|
|
std::cout << "===============config配置内容===============" << std::endl;
|
|
|
std::cout << " 雷达IP: " << Leida_IP << std::endl;
|
|
|
std::cout << " 端口: " << Leida_PORT << std::endl;
|
|
|
std::cout << " 采集时间: " << Collect_Duration_S << " s" << std::endl;
|
|
|
std::cout << " 运行速度: " << Speed_Z << " cm/s" << std::endl;
|
|
|
std::cout << " Z截面区间: " << Z_RANGE_MM << " mm" << std::endl;
|
|
|
std::cout << " Y截面区间: " << Y_RANGE_MM << " mm (从Y最小开始)" << std::endl;
|
|
|
std::cout << " 雷达帧率: " << Lidar_Hz << " Hz" << std::endl;
|
|
|
std::cout << " 矩形宽度: " << Rect_Width_PX << " px" << std::endl;
|
|
|
std::cout << " 矩形长倍率: " << Rect_Length_Ratio << std::endl;
|
|
|
std::cout << " 运行模式: " << Run_Mode << " (tcp端口: " << Tcp_Port << ")" << std::endl;
|
|
|
std::cout << " 日志使能: " << (Log_Flag ? "YES" : "NO") << std::endl;
|
|
|
std::cout << " 地板高度(Floor_Height): " << Floor_Height << " mm (物品高度=Floor_Height-Y轴)" << std::endl;
|
|
|
std::cout << "========================================" << std::endl;
|
|
|
|
|
|
std::cout << "========================================" << std::endl;
|
|
|
std::cout << " Pavo2 Lidar 雷达数据采集程序" << std::endl;
|
|
|
std::cout << " SDK版本: " << SDK_VER << std::endl;
|
|
|
std::cout << "========================================" << std::endl;
|
|
|
|
|
|
pavo2::pavo2_driver driver;
|
|
|
|
|
|
if (!connect_lidar(driver))
|
|
|
{
|
|
|
std::cerr << "\n[退出] 连接雷达失败，程序退出。" << std::endl;
|
|
|
std::cout << "请检查：" << std::endl;
|
|
|
std::cout << " 1. 雷达IP地址是否正确（当前: " << Leida_IP << "）" << std::endl;
|
|
|
std::cout << " 2. 本机与雷达的网络是否连通" << std::endl;
|
|
|
std::cout << " 3. 防火墙是否放行端口 " << Leida_PORT << std::endl;
|
|
|
#ifdef _WIN32
|
|
|
WSACleanup();
|
|
|
#endif
|
|
|
return -1;
|
|
|
}
|
|
|
|
|
|
std::cout << "\n[等待] 等待雷达数据稳定（2秒）..." << std::endl;
|
|
|
std::this_thread::sleep_for(std::chrono::seconds(2));
|
|
|
|
|
|
// --- 功能1：单帧 + 鸟瞰图 (已关闭) ---
|
|
|
// demo_single_frame(driver);
|
|
|
|
|
|
// --- 运行模式: exe=直接检测一次; tcp=服务端模式，收到 {"check":1} 触发检测 ---
|
|
|
if (Run_Mode == "tcp")
|
|
|
{
|
|
|
run_tcp_server(driver);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
// --- 功能2：多秒采集 + 深度图 ---
|
|
|
demo_collect_multi(driver);
|
|
|
}
|
|
|
|
|
|
std::cout << "\n[关闭] 正在断开雷达连接..." << std::endl;
|
|
|
driver.pavo2_close();
|
|
|
|
|
|
#ifdef _WIN32
|
|
|
WSACleanup();
|
|
|
#endif
|
|
|
|
|
|
std::cout << "[完成] 程序运行结束。" << std::endl;
|
|
|
return 0;
|
|
|
}
|