upright
/
leida_simi
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
main
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '0af49ebfe7'
${ noResults }
leida_simi/main_legacy.cpp

1844 lines
80 KiB
C++
Raw Normal View History Unescape Escape

基础包
2 months ago
// 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;
}