Skip to content

Commit

Permalink
修改三角和GS驱动类线程创建方式
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhanyiaini
zhanyiaini committed Oct 9, 2024
1 parent 68a4d76 commit ad8e30f
Show file tree
Hide file tree
Showing 6 changed files with 2,153 additions and 1,970 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ project(ydlidar_sdk C CXX)
# version
set(YDLIDAR_SDK_VERSION_MAJOR 1)
set(YDLIDAR_SDK_VERSION_MINOR 2)
set(YDLIDAR_SDK_VERSION_PATCH 6)
set(YDLIDAR_SDK_VERSION_PATCH 7)
set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})

##########################################################
Expand Down
30 changes: 16 additions & 14 deletions core/common/DriverInterface.h
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
#pragma once
#include <map>
#include <thread>
#include <core/base/v8stdint.h>
#include <core/base/thread.h>
#include <core/base/locker.h>
#include <core/base/datatype.h>
#include <map>
#include "ydlidar_protocol.h"
#include "ydlidar_def.h"

Expand Down Expand Up @@ -552,8 +553,8 @@ namespace ydlidar
{
DEFAULT_TIMEOUT = 2000, /**< Default timeout. */
DEFAULT_HEART_BEAT = 1000, /**< Default heartbeat timeout. */
MAX_SCAN_NODES = 7200, /**< Default Max Scan Count. */
DEFAULT_TIMEOUT_COUNT = 1, /**< Default Timeout Count. */
MAX_SCAN_NODES = 5000, /**< Default Max Scan Count. */
DEFAULT_TIMEOUT_COUNT = 2, /**< Default Timeout Count. */
};

protected:
Expand All @@ -567,7 +568,8 @@ namespace ydlidar
/// Data Locker（不支持嵌套）
Locker _lock;
/// Parse Data thread
Thread _thread;
Thread _thread; //线程对象
std::thread* m_thread = nullptr; //STD线程对象
/// command locker（不支持嵌套）
Locker _cmd_lock;
/// driver error locker（不支持嵌套）
Expand All @@ -578,30 +580,30 @@ namespace ydlidar
/// baudrate or IP port
uint32_t m_baudrate;
/// LiDAR intensity
bool m_intensities;
bool m_intensities = false;
/// LiDAR intensity bit
int m_intensityBit;
int m_intensityBit = 0;

/// LiDAR Point pointer
node_info *scan_node_buf;
node_info *scan_node_buf = nullptr;
/// LiDAR scan count
size_t scan_node_count; //<! LiDAR Scan Count
size_t scan_node_count = 0; //LiDAR Scan Count
/// package sample index
uint16_t nodeIndex;
uint16_t nodeIndex = 0;
///
int retryCount;
int retryCount = 0;
/// auto reconnect
bool isAutoReconnect;
bool isAutoReconnect = true;
/// auto connecting state
bool isAutoconnting;
bool isAutoconnting = false;
lidarConfig m_config;

/// number of last error
DriverError m_driverErrno;

/// invalid node count
int m_InvalidNodeCount;
size_t m_BufferSize;
int m_InvalidNodeCount = 0;
size_t m_BufferSize = 0;
};

} // common
Expand Down
2 changes: 1 addition & 1 deletion examples/tri_and_gs_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,7 @@ int main(int argc, char *argv[])
lidarS2.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int));
//////////////////////bool property/////////////////
/// fixed angle resolution
bool b_optvalue = true;
bool b_optvalue = false;
lidarS2.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
b_optvalue = false;
/// rotate 180
Expand Down
18 changes: 16 additions & 2 deletions src/CYdLidar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -444,9 +444,22 @@ bool CYdLidar::initialize()
/*-------------------------------------------------------------
initialize
-------------------------------------------------------------*/
void CYdLidar::GetLidarVersion(LidarVersion &version)
void CYdLidar::GetLidarVersion(LidarVersion &lv)
{
memcpy(&version, &m_LidarVersion, sizeof(LidarVersion));
memcpy(&lv, &m_LidarVersion, sizeof(LidarVersion));

printf("[YDLIDAR] lidar version\n"
"Firmware version: %u.%u.%u\n"
"Hardware version: %u\n"
"Serial: ",
lv.soft_major,
lv.soft_minor,
lv.soft_patch,
lv.hardware);
for (int i = 0; i < SDK_SNLEN; i++)
printf("%01X", lv.sn[i] & 0xff);
printf("\n");
fflush(stdout);
}

/*-------------------------------------------------------------
Expand Down Expand Up @@ -1127,6 +1140,7 @@ bool CYdLidar::checkLidarAbnormal()
// 获取CT信息
if (!(lidarPtr->getHasDeviceInfo() & EPT_Module))
{
// printf("Get module device info\n");
LaserDebug debug = {0};
for (int i = 0; i < count; ++i)
{
Expand Down
94 changes: 43 additions & 51 deletions src/GSLidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,6 @@ GSLidarDriver::GSLidarDriver(uint8_t type)
//串口配置参数
m_intensities = false;
isAutoReconnect = true;
isAutoconnting = false;
m_baudrate = 230400;
scan_node_count = 0;
sample_rate = 5000;
Expand All @@ -81,16 +80,19 @@ GSLidarDriver::GSLidarDriver(uint8_t type)
nodeIndex = 0;
globalRecvBuffer = new uint8_t[GSPACKSIZE];
scan_node_buf = new node_info[MAX_SCAN_NODES];
bias[0] = 0;
bias[1] = 0;
bias[2] = 0;
for (int i=0; i<LIDAR_MAXCOUNT; ++i)
{
k0[i] = 0;
k1[i] = 0;
b0[i] = 0;
b1[i] = 0;
bias[i] = 0;
}
}

GSLidarDriver::~GSLidarDriver()
{
m_isScanning = false;
isAutoReconnect = false;
_thread.join();
disableDataGrabbing();

ScopedLocker l(_cmd_lock);
if (_comm) {
Expand Down Expand Up @@ -202,13 +204,20 @@ void GSLidarDriver::disconnect() {
m_isConnected = false;
}

void GSLidarDriver::disableDataGrabbing()
void GSLidarDriver::disableDataGrabbing()
{
if (m_isScanning) {
m_isScanning = false;
_dataEvent.set();
}
_thread.join();
if (m_thread)
{
if (m_thread->joinable())
m_thread->join();
delete m_thread;
m_thread = nullptr;
}
// _thread.join();
}

bool GSLidarDriver::isscanning() const {
Expand Down Expand Up @@ -524,59 +533,39 @@ result_t GSLidarDriver::waitForData(size_t data_count, uint32_t timeout,
result_t GSLidarDriver::checkAutoConnecting()
{
result_t ans = RESULT_FAIL;
isAutoconnting = true;

if (m_driverErrno != BlockError)
setDriverError(TimeoutError);

while (isAutoReconnect && isAutoconnting)
while (isAutoReconnect && isscanning())
{
{
ScopedLocker l(_cmd_lock);
if (_comm) {
if (_comm->isOpen() || m_isConnected) {
if (_comm->isOpen()) {
m_isConnected = false;
_comm->closePort();
delete _comm;
_comm = NULL;
}
}
}
retryCount ++;
if (retryCount > 100)
retryCount = 100;

delay(100);
delay(100); //延时

int retryConnect = 0;
while (isAutoReconnect &&
while (isscanning() &&
connect(m_port.c_str(), m_baudrate) != RESULT_OK)
{
setDriverError(NotOpenError);
retryConnect ++;
if (retryConnect > 5)
retryConnect = 5;

delay(200);
delay(300);
}

if (!isAutoReconnect) {
m_isScanning = false;
if (!isscanning()) {
return RESULT_FAIL;
}
//判断是否已重连，如是则尝试启动雷达
if (isconnected())
{
delay(100);
{
ans = startAutoScan();
if (!IS_OK(ans)) {
ans = startAutoScan();
}
}

ans = startAutoScan();
if (IS_OK(ans)) {
isAutoconnting = false;
return ans;
}
else {
Expand Down Expand Up @@ -618,12 +607,12 @@ int GSLidarDriver::cacheScanData()
if (m_driverErrno != BlockError)
setDriverError(TimeoutError);
}
fprintf(stderr, "[YDLIDAR] Timeout count: %d\n", timeout_count);
fprintf(stderr, "[GSLIDAR] Timeout count: %d\n", timeout_count);
fflush(stderr);
// 重连雷达
if (!isAutoReconnect)
{
fprintf(stderr, "[YDLIDAR] Exit scanning thread!!\n");
fprintf(stderr, "[GSLIDAR] Exit scanning thread\n");
fflush(stderr);
m_isScanning = false;
return RESULT_FAIL;
Expand Down Expand Up @@ -845,7 +834,8 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout)
if (CheckSumResult)
{
model = m_models[moduleNum]; //当前雷达型号
printf("Module[%d] Lidar model[%u]\n", moduleNum, model);
if (m_Debug)
printf("GS Lidar Module[%d] Model[%u]\n", moduleNum, model);
//根据雷达型号设置角度参数
if (YDLIDAR_GS5 == model)
m_pitchAngle = Angle_PAngle2;
Expand Down Expand Up @@ -1466,19 +1456,25 @@ result_t GSLidarDriver::stopScan(uint32_t timeout)
result_t GSLidarDriver::createThread()
{
// 如果线程已启动，则先退出线程
if (_thread.getHandle())
// if (_thread.getHandle())
// {
// m_isScanning = false;
// _thread.join();
// }
// _thread = CLASS_THREAD(GSLidarDriver, cacheScanData);
// if (!_thread.getHandle()) {
// return RESULT_FAIL;
// }
m_thread = new std::thread(&GSLidarDriver::cacheScanData, this);
if (!m_thread)
{
m_isScanning = false;
_thread.join();
}
_thread = CLASS_THREAD(GSLidarDriver, cacheScanData);
if (!_thread.getHandle()) {
printf("[GSLidar] Fail to create GS thread\n");
fflush(stdout);
return RESULT_FAIL;
}

printf("[GS2Lidar] Create GS thread 0x%X\n", _thread.getHandle());
printf("[GSLidar] Create GS thread 0x%X\n", m_thread->get_id());
fflush(stdout);

return RESULT_OK;
}

Expand Down Expand Up @@ -1514,10 +1510,6 @@ result_t GSLidarDriver::startAutoScan(bool force, uint32_t timeout) {
/************************************************************************/
result_t GSLidarDriver::stop()
{
if (isAutoconnting) {
isAutoReconnect = false;
}

disableDataGrabbing();
stopScan();

Expand Down
Loading

0 comments on commit ad8e30f

Please sign in to comment.