Skip to content

Commit 2b7e756

Browse files
zhanyiainizhanyiaini
committed
修改GS2和S2系列同时使用时线程异常问题
1 parent 66c77ca commit 2b7e756

File tree

8 files changed

+519
-248
lines changed

8 files changed

+519
-248
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@ project(ydlidar_sdk C CXX)
55
# version
66
set(YDLIDAR_SDK_VERSION_MAJOR 1)
77
set(YDLIDAR_SDK_VERSION_MINOR 1)
8-
set(YDLIDAR_SDK_VERSION_PATCH 2)
8+
set(YDLIDAR_SDK_VERSION_PATCH 3)
99
set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})
1010

1111
##########################################################

‎core/base/thread.h‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -107,19 +107,17 @@ class Thread {
107107
void *res;
108108
int s = -1;
109109
s = pthread_cancel((pthread_t)(this->_handle));
110-
111110
if (s != 0) {
112111
}
113112

114113
s = pthread_join((pthread_t)(this->_handle), &res);
115-
116114
if (s != 0) {
117115
}
118116

119117
if (res == PTHREAD_CANCELED) {
120-
printf("%lu thread has been canceled\n", this->_handle);
121-
this->_handle = 0;
118+
printf("0x%X thread has been canceled\n", this->_handle);
122119
}
120+
this->_handle = 0; //强制置空线程句柄，以免再次调用该函数时出现异常
123121

124122
#endif
125123
return 0;

‎samples/gs_test.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,7 @@ int main(int argc, char *argv[])
7979
int id = 0;
8080

8181
for (it = ports.begin(); it != ports.end(); it++) {
82-
printf("%d. %s\n", id, it->first.c_str());
82+
printf("[%d] %s %s\n", id, it->first.c_str(), it->second.c_str());
8383
id++;
8484
}
8585

‎samples/tri_and_gs_test.cpp‎

Lines changed: 237 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,237 @@
1+
#include "CYdLidar.h"
2+
#include <iostream>
3+
#include <string>
4+
#include <algorithm>
5+
#include <cctype>
6+
#include "core/base/timer.h"
7+
8+
using namespace std;
9+
using namespace ydlidar;
10+
11+
#if defined(_MSC_VER)
12+
#pragma comment(lib, "ydlidar_sdk.lib")
13+
#endif
14+
15+
int main(int argc, char *argv[])
16+
{
17+
printf("__ ______ _ ___ ____ _ ____ \n");
18+
printf("\\ \\ / / _ \\| | |_ _| _ \\ / \\ | _ \\ \n");
19+
printf(" \\ V /| | | | | | || | | |/ _ \\ | |_) | \n");
20+
printf(" | | | |_| | |___ | || |_| / ___ \\| _ < \n");
21+
printf(" |_| |____/|_____|___|____/_/ \\_\\_| \\_\\ \n");
22+
printf("\n");
23+
fflush(stdout);
24+
25+
ydlidar::os_init();
26+
27+
bool ret = false;
28+
CYdLidar lidarGs; //GS2雷达
29+
{
30+
bool isSingleChannel = false;
31+
float frequency = 8.0;
32+
std::string port = "/dev/ttyUSB0";
33+
int baudrate = 921600;
34+
//////////////////////string property/////////////////
35+
/// lidar port
36+
lidarGs.setlidaropt(LidarPropSerialPort, port.c_str(), port.size());
37+
/// lidar baudrate
38+
lidarGs.setlidaropt(LidarPropSerialBaudrate, &baudrate, sizeof(int));
39+
/// gs lidar
40+
int optval = TYPE_GS;
41+
lidarGs.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
42+
/// device type
43+
optval = YDLIDAR_TYPE_SERIAL;
44+
lidarGs.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
45+
/// sample rate
46+
optval = 4;
47+
lidarGs.setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
48+
/// abnormal count
49+
optval = 4;
50+
lidarGs.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));
51+
/// Intenstiy bit count
52+
optval = 8;
53+
lidarGs.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int));
54+
//////////////////////bool property/////////////////
55+
/// fixed angle resolution
56+
bool b_optvalue = false;
57+
lidarGs.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
58+
/// rotate 180
59+
lidarGs.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
60+
/// Counterclockwise
61+
lidarGs.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
62+
b_optvalue = true;
63+
lidarGs.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
64+
/// one-way communication
65+
lidarGs.setlidaropt(LidarPropSingleChannel, &isSingleChannel, sizeof(bool));
66+
/// intensity
67+
b_optvalue = true;
68+
lidarGs.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
69+
/// Motor DTR
70+
b_optvalue = true;
71+
lidarGs.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));
72+
/// HeartBeat
73+
b_optvalue = false;
74+
lidarGs.setlidaropt(LidarPropSupportHeartBeat, &b_optvalue, sizeof(bool));
75+
//////////////////////float property/////////////////
76+
/// unit: °
77+
float f_optvalue = 180.0f;
78+
lidarGs.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
79+
f_optvalue = -180.0f;
80+
lidarGs.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float));
81+
/// unit: m
82+
f_optvalue = 1.f;
83+
lidarGs.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
84+
f_optvalue = 0.025f;
85+
lidarGs.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
86+
/// unit: Hz
87+
lidarGs.setlidaropt(LidarPropScanFrequency, &frequency, sizeof(float));
88+
89+
//雷达初始化
90+
ret = lidarGs.initialize();
91+
if (!ret)
92+
{
93+
fprintf(stderr, "Fail to initialize %s\n", lidarGs.DescribeError());
94+
fflush(stderr);
95+
return -1;
96+
}
97+
}
98+
99+
CYdLidar lidarS2; //S2雷达
100+
{
101+
bool isSingleChannel = false;
102+
float frequency = 8.0;
103+
std::string port = "/dev/ttyUSB1";
104+
int baudrate = 115200;
105+
//////////////////////string property/////////////////
106+
/// lidar port
107+
lidarS2.setlidaropt(LidarPropSerialPort, port.c_str(), port.size());
108+
//////////////////////int property/////////////////
109+
/// lidar baudrate
110+
lidarS2.setlidaropt(LidarPropSerialBaudrate, &baudrate, sizeof(int));
111+
/// tof lidar
112+
int optval = TYPE_TRIANGLE;
113+
lidarS2.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
114+
/// device type
115+
optval = YDLIDAR_TYPE_SERIAL;
116+
lidarS2.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
117+
/// sample rate
118+
optval = isSingleChannel ? 3 : 4;
119+
lidarS2.setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
120+
/// abnormal count
121+
optval = 4;
122+
lidarS2.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));
123+
/// Intenstiy bit count
124+
optval = 10;
125+
lidarS2.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int));
126+
//////////////////////bool property/////////////////
127+
/// fixed angle resolution
128+
bool b_optvalue = true;
129+
lidarS2.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
130+
b_optvalue = false;
131+
/// rotate 180
132+
lidarS2.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
133+
/// Counterclockwise
134+
lidarS2.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
135+
b_optvalue = true;
136+
lidarS2.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
137+
/// one-way communication
138+
lidarS2.setlidaropt(LidarPropSingleChannel, &isSingleChannel, sizeof(bool));
139+
/// intensity
140+
b_optvalue = true;
141+
lidarS2.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
142+
/// Motor DTR
143+
b_optvalue = false;
144+
lidarS2.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));
145+
/// HeartBeat
146+
b_optvalue = false;
147+
lidarS2.setlidaropt(LidarPropSupportHeartBeat, &b_optvalue, sizeof(bool));
148+
//////////////////////float property/////////////////
149+
/// unit: °
150+
float f_optvalue = 180.0f;
151+
lidarS2.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
152+
f_optvalue = -180.0f;
153+
lidarS2.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float));
154+
/// unit: m
155+
f_optvalue = 64.f;
156+
lidarS2.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
157+
f_optvalue = 0.05f;
158+
lidarS2.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
159+
/// unit: Hz
160+
lidarS2.setlidaropt(LidarPropScanFrequency, &frequency, sizeof(float));
161+
ret = lidarS2.initialize();
162+
if (!ret)
163+
{
164+
fprintf(stderr, "Fail to initialize %s\n", lidarS2.DescribeError());
165+
fflush(stderr);
166+
return -1;
167+
}
168+
}
169+
170+
LaserScan scanGs; //GS2点云数据
171+
LaserScan scanS2; //S2雷达点云数据
172+
while (ydlidar::os_isOk())
173+
{
174+
//启动S2
175+
ret = lidarS2.turnOn();
176+
if (!ret)
177+
{
178+
fprintf(stderr, "Fail to turn on S2 %s\n", lidarS2.DescribeError());
179+
fflush(stderr);
180+
return -1;
181+
}
182+
//启动GS2
183+
ret = lidarGs.turnOn();
184+
if (!ret)
185+
{
186+
fprintf(stderr, "Fail to turn on GS2 %s\n", lidarGs.DescribeError());
187+
fflush(stderr);
188+
return -1;
189+
}
190+
//启动后运行5秒然后停止扫描
191+
uint64_t t = getms();
192+
while (getms() - t < 5000)
193+
{
194+
//获取GS2点云数据
195+
if (lidarGs.doProcessSimple(scanGs))
196+
{
197+
printf("[%lu] points in [0x%016lX] module num [%d] env flag [0x%04X]\n",
198+
scanGs.points.size(),
199+
scanGs.stamp,
200+
scanGs.moduleNum,
201+
scanGs.envFlag);
202+
fflush(stdout);
203+
}
204+
else
205+
{
206+
fprintf(stderr, "Failed to get Lidar GS2 Data\n");
207+
fflush(stderr);
208+
}
209+
//获取S2点云数据
210+
if (lidarS2.doProcessSimple(scanS2))
211+
{
212+
printf("[%u] points inc [%f]\n",
213+
(unsigned int)scanS2.points.size(),
214+
scanS2.config.angle_increment);
215+
fflush(stdout);
216+
}
217+
else
218+
{
219+
fprintf(stderr, "Failed to get Lidar S2 Data\n");
220+
fflush(stderr);
221+
static int s_errorCount = 0;
222+
if (s_errorCount++ > 10)
223+
return -1;
224+
}
225+
}
226+
227+
//停止S2
228+
lidarS2.turnOff();
229+
//停止GS2
230+
lidarGs.turnOff();
231+
}
232+
233+
lidarGs.turnOff();
234+
lidarGs.disconnecting();
235+
236+
return 0;
237+
}

‎src/CYdLidar.cpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1088,6 +1088,7 @@ bool CYdLidar::checkLidarAbnormal()
10881088
}
10891089
}
10901090

1091+
//单通雷达，计算采样率、固定分辨率时的一圈点数
10911092
if (IS_OK(op_result) && lidarPtr->getSingleChannel())
10921093
{
10931094
data.push_back(count);
@@ -1115,7 +1116,7 @@ bool CYdLidar::checkLidarAbnormal()
11151116
scan_time = 1.0 * static_cast<int64_t>(end_time - start_time) / 1e9;
11161117
bool ret = CalculateSampleRate(count, scan_time);
11171118

1118-
if (scan_time > 0.05 && scan_time < 0.5 && lidarPtr->getSingleChannel())
1119+
if (scan_time > 0.05 && scan_time < 0.5)
11191120
{
11201121
if (!ret)
11211122
{
@@ -1717,7 +1718,8 @@ bool CYdLidar::checkScanFrequency()
17171718

17181719
m_ScanFrequency -= frequencyOffset;
17191720
m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1);
1720-
printf("[YDLIDAR INFO] Current Scan Frequency: %fHz\n", m_ScanFrequency);
1721+
printf("[YDLIDAR] Current Scan Frequency: %fHz\n", m_ScanFrequency);
1722+
printf("[YDLIDAR] Fixed size: %d\n", m_FixedSize);
17211723
return true;
17221724
}
17231725

0 commit comments

Comments
 (0)