Skip to content

Commit

Permalink
针对时间戳协议进行修改
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhanyiaini
zhanyiaini committed Feb 27, 2024
1 parent 3c40008 commit b73fe49
Show file tree
Hide file tree
Showing 9 changed files with 329 additions and 157 deletions.
4 changes: 2 additions & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,8 @@ project(ydlidar_sdk C CXX)
#########################################################
# version
set(YDLIDAR_SDK_VERSION_MAJOR 1)
set(YDLIDAR_SDK_VERSION_MINOR 1)
set(YDLIDAR_SDK_VERSION_PATCH 23)
set(YDLIDAR_SDK_VERSION_MINOR 2)
set(YDLIDAR_SDK_VERSION_PATCH 0)
set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})

##########################################################
Expand Down
72 changes: 71 additions & 1 deletion core/common/ydlidar_help.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
#include "DriverInterface.h"
#include <sstream>
#include <iomanip>
#include <stdarg.h>

/**
* @brief ydlidar
Expand Down Expand Up @@ -1024,13 +1025,82 @@ inline bool isV1Protocol(uint8_t protocol) {
//以16进制打印数据
inline void printHex(const uint8_t *data, int size)
{
if (!data)
if (!data || !size)
return;
for (int i=0; i<size; ++i)
printf("%02X", data[i]);
printf("\n");
}

//打印系统时间
#define UNIX_PRINT_TIME \
time_t currentTime = time(NULL); \
struct tm *localTime = localtime(&currentTime); \
printf("[%04d-%02d-%02d %02d:%02d:%02d]", \
(1900 + localTime->tm_year), \
(1 + localTime->tm_mon), \
localTime->tm_mday, \
localTime->tm_hour, \
localTime->tm_min, \
localTime->tm_sec);
//格式化字符串
#define FORMAT_STDOUT \
char buff[1024] = {0}; \
va_list ap; \
va_start(ap, fmt); \
vsprintf(buff, fmt, ap); \
va_end(ap); \
printf(buff); \
printf("\n");

//调试
inline void debug(char* fmt, ...)
{
#ifdef _WIN32
#else
UNIX_PRINT_TIME
#endif
printf("[debug] ");
FORMAT_STDOUT
fflush(stdout);
}

//常规
inline void info(char* fmt, ...)
{
#ifdef _WIN32
#else
UNIX_PRINT_TIME
#endif
printf("[info] ");
FORMAT_STDOUT
fflush(stdout);
}

//警告
inline void warn(char* fmt, ...)
{
#ifdef _WIN32
#else
UNIX_PRINT_TIME
#endif
printf("[warn] ");
FORMAT_STDOUT
fflush(stdout);
}

//错误
inline void error(char* fmt, ...)
{
#ifdef _WIN32
#else
UNIX_PRINT_TIME
#endif
printf("[error] ");
FORMAT_STDOUT
fflush(stdout);
}

}//common
}//core
}//ydlidar
39 changes: 15 additions & 24 deletions core/common/ydlidar_protocol.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,21 +137,14 @@
#define LIDAR_MODULE_3 0x04
#define LIDAR_MODULE_ALL 0x00
#define LIDAR_MAXCOUNT 3 //最大模组数
#define GSMAXPOINTCOUNT 160 //GS数据包中最大点云数
#define LIDAR_PACKMAXPOINTSIZE 160 //单包最大点数

//GS
#define Angle_Px 1.22
#define Angle_Py 5.315
#define Angle_PAngle 22.5
#define PackageMaxModuleNums 0x03
#define GS_MAXPOINTS 160 //GS2固定160个点
#define MaxPointsPerPackge_GS1 216 //GS1固定216个点
#define PackagePaidBytes_GS 8
#define NORMAL_PACKAGE_SIZE 331

/// Maximuum number of samples in a packet
#define PackageSampleMaxLngth 0x100
#define MaximumNumberOfPackages 765 //= 255 * 3
#define Angle_Px 1.22
#define Angle_Py 5.315
#define Angle_PAngle 22.5
#define GS_PACKHEADSIZE 8
#define GS_MAXPOINTSIZE 160 //GS数据包中最大点云数

#define SDK_SNLEN 16 //序列号长度

Expand All @@ -161,18 +154,16 @@
#define Node_Sync 1
/// Normal Node
#define Node_NotSync 2
/// Package Header Size
#define PackagePaidBytes 10
/// Package Header
#define PH 0x55AA
#define PH1 0xAA
#define PH2 0x55 //AA55是点云数据
#define PH3 0x66 //AA66是时间戳数据

/// Package Header Size
#define TRI_PACKHEADSIZE 10
/// Normal Package size
#define TrianglePackageDataSize 40
/// TOF Normal package size
#define TOFPackageDataSize 80
#define TRI_PACKDATASIZE 40

#define FREINDEX 0
#define USERVERSIONNDEX 1
Expand Down Expand Up @@ -252,7 +243,7 @@ struct tri_node_package {
uint16_t firstAngle;///< first sample angle
uint16_t lastAngle;///< last sample angle
uint16_t cs;///< checksum
uint16_t nodes[PackageSampleMaxLngth];
uint16_t nodes[LIDAR_PACKMAXPOINTSIZE];
} __attribute__((packed));

//LiDAR Intensity Nodes Package
Expand All @@ -263,7 +254,7 @@ struct tri_node_package2 {
uint16_t firstAngle;///< first sample angle
uint16_t lastAngle;///< last sample angle
uint16_t cs;///< checksum
tri_node2 nodes[PackageSampleMaxLngth];
tri_node2 nodes[LIDAR_PACKMAXPOINTSIZE];
} __attribute__((packed));

// TOF LiDAR Intensity Nodes Package
Expand All @@ -274,7 +265,7 @@ struct tof_node_package {
uint16_t firstAngle;
uint16_t lastAngle;
uint16_t cs;
tof_node nodes[PackageSampleMaxLngth];
tof_node nodes[LIDAR_PACKMAXPOINTSIZE];
} __attribute__((packed));

//时间戳结构体
Expand Down Expand Up @@ -369,12 +360,12 @@ struct gs_packages {
int moduleNum;
bool left = false;
bool right = false;
node_info points[GS_MAXPOINTS];
node_info points[GS_MAXPOINTSIZE];
} __attribute__((packed));
struct gs_module_nodes {
int moduleNum = 0;
int pointCount = 0;
node_info points[GS_MAXPOINTS];
node_info points[GS_MAXPOINTSIZE];
} __attribute__((packed));

//GS点数据结构
Expand All @@ -392,7 +383,7 @@ struct gs_node_package {
uint8_t ct;
uint16_t size;
uint16_t env;
gs_node nodes[GS_MAXPOINTS];
gs_node nodes[GS_MAXPOINTSIZE];
uint8_t cs;
} __attribute__((packed));
#define GSPACKSIZE sizeof(gs_node_package) //定义GS点大小
Expand Down
121 changes: 91 additions & 30 deletions examples/tea_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
#include <algorithm>
#include <cctype>
#include "CYdLidar.h"
#include "core/common/ydlidar_help.h"
#include "filters/NoiseFilter.h"
#include "filters/StrongLightFilter.h"

Expand All @@ -46,6 +47,7 @@ using namespace ydlidar;
#pragma comment(lib, "ydlidar_sdk.lib")
#endif


int main(int argc, char *argv[])
{
printf("__ ______ _ ___ ____ _ ____ \n");
Expand Down Expand Up @@ -99,34 +101,78 @@ int main(int argc, char *argv[])
printf("Please enter the lidar IP: ");
std::cin >> port;
}
}

int baudrate = 8090;
//串口
// ports = ydlidar::lidarPortList();
// if (ports.size() == 1)
// {
// port = ports.begin()->second;
// }
// else
// {
// int id = 0;

if (!ydlidar::os_isOk())
{
return 0;
}
// for (it = ports.begin(); it != ports.end(); it++)
// {
// printf("[%d] %s %s\n", id, it->first.c_str(), it->second.c_str());
// id++;
// }

bool isSingleChannel = false;
// if (ports.empty())
// {
// printf("Not Lidar was detected. Please enter the lidar serial port:");
// std::cin >> port;
// }
// else
// {
// while (ydlidar::os_isOk())
// {
// printf("Please select the lidar port:");
// std::string number;
// std::cin >> number;

std::string input_frequency;
// if ((size_t)atoi(number.c_str()) >= ports.size())
// {
// continue;
// }

float frequency = 20.0f;
// it = ports.begin();
// id = atoi(number.c_str());

while (ydlidar::os_isOk() && !isSingleChannel)
{
printf("Please input the lidar scan frequency[10-30]: ");
std::cin >> input_frequency;
frequency = atof(input_frequency.c_str());
if (frequency <= 30.0 && frequency >= 10.0)
{
break;
}
// while (id)
// {
// id--;
// it++;
// }

// port = it->second;
// break;
// }
// }
// }

fprintf(stderr, "Invalid scan frequency Please re-input.\n");
}

// int baudrate = 512000; //8090
int baudrate = 8090;

bool isSingleChannel = false;

float frequency = 20.0f;

// std::string input_frequency;
// while (ydlidar::os_isOk() && !isSingleChannel)
// {
// printf("Please input the lidar scan frequency[10-30]: ");
// std::cin >> input_frequency;
// frequency = atof(input_frequency.c_str());
// if (frequency <= 30.0 && frequency >= 10.0)
// {
// break;
// }
// fprintf(stderr, "Invalid scan frequency Please re-input.\n");
// }

/// instance
CYdLidar laser;
//////////////////////string property/////////////////
Expand All @@ -145,6 +191,7 @@ int main(int argc, char *argv[])
int optval = TYPE_TOF;
laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
/// device type
// optval = YDLIDAR_TYPE_SERIAL;
optval = YDLIDAR_TYPE_TCP;
laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
/// sample rate
Expand All @@ -158,7 +205,7 @@ int main(int argc, char *argv[])

//////////////////////bool property/////////////////
/// fixed angle resolution
bool b_optvalue = true;
bool b_optvalue = false;
laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
/// rotate 180
b_optvalue = false;
Expand Down Expand Up @@ -191,6 +238,8 @@ int main(int argc, char *argv[])
/// unit: Hz
laser.setlidaropt(LidarPropScanFrequency, &frequency, sizeof(float));

// laser.setEnableDebug(true); //启用调试

/// initialize SDK and LiDAR.
bool ret = laser.initialize();

Expand All @@ -209,23 +258,35 @@ int main(int argc, char *argv[])
LaserScan outScan;
StrongLightFilter filter; //拖尾滤波器
filter.setMaxDist(0.1); //最大距离阈值
filter.setMaxAngle(12.0); //最大角度阈值
filter.setMinNoise(2); //最小连续噪点数

float minScanTime = 1.0f / (frequency + 2.5);
float maxScanTime = 1.0f / (frequency - 2.5);
while (ret && ydlidar::os_isOk())
{
//循环获取点云数据
if (laser.doProcessSimple(scan))
{
fprintf(stdout, "Scan received [%llu] points is [%f]s [%f]\n",
scan.points.size(),
scan.config.scan_time,
scan.config.time_increment);
fflush(stdout);
for (size_t i = 0; i < scan.points.size(); ++i)
{
const LaserPoint &p = scan.points.at(i);
printf("%d d %.0f a %.02f i %.0f\n", i, p.range, p.angle * 180.0 / M_PI, p.intensity);
}
// if (scan.config.scan_time < minScanTime ||
// scan.config.scan_time > maxScanTime)
// {
// core::common::error("[%u] points ScanTime [%f]s IncrTime [%f]s",
// uint32_t(scan.points.size()),
// scan.config.scan_time,
// scan.config.time_increment);
// }

// core::common::info("[%u] points Stamp [%u]ms",
// uint32_t(scan.points.size()),
// uint32_t(scan.stamp / 1000000));

// for (size_t i = 0; i < scan.points.size(); ++i)
// {
// const LaserPoint &p = scan.points.at(i);
// printf("%d d %.0f a %.02f i %.0f\n", i, p.range, p.angle * 180.0 / M_PI, p.intensity);
// }
// fflush(stdout);

// 使用强光滤波器
// filter.filter(scan, 0, 0, outScan);
Expand Down
Loading

0 comments on commit b73fe49

Please sign in to comment.