Skip to content

Commit

Permalink
1.feature(*): add lidar error infomation.
Browse files Browse the repository at this point in the history 
2.fix(*): fix ignore array.
3.feate(*): update version to 1.0.2
  • Loading branch information
@yangfuyuan
yangfuyuan committed Sep 19, 2020
1 parent 8883121 commit 6dc2502
Show file tree
Hide file tree
Showing 11 changed files with 499 additions and 205 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 0)
set(YDLIDAR_SDK_VERSION_PATCH 1)
set(YDLIDAR_SDK_VERSION_PATCH 2)
set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})

##########################################################
Expand Down
2 changes: 1 addition & 1 deletion doc/howto/how_to_build_and_install.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ You can install these packages using apt:
```shell
sudo apt install cmake pkg-config
```
if you want to use python API, you need to install pyhton and swig:
if you want to use python API, you need to install pyhton and swig(3.0 or higher):
```shell
sudo apt-get install python swig
sudo apt-get install python-pip
Expand Down
4 changes: 4 additions & 0 deletions python/examples/plot_tof_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,10 @@
laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0);
laser.setlidaropt(ydlidar.LidarPropSampleRate, 20);
laser.setlidaropt(ydlidar.LidarPropSingleChannel, False);
laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0);
laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0);
laser.setlidaropt(ydlidar.LidarPropMaxRange, 32.0);
laser.setlidaropt(ydlidar.LidarPropMinRange, 0.01);
scan = ydlidar.LaserScan()

def animate(num):
Expand Down
4 changes: 4 additions & 0 deletions python/examples/ydlidar_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,10 @@
laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0);
laser.setlidaropt(ydlidar.LidarPropSampleRate, 9);
laser.setlidaropt(ydlidar.LidarPropSingleChannel, False);
laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0);
laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0);
laser.setlidaropt(ydlidar.LidarPropMaxRange, 16.0);
laser.setlidaropt(ydlidar.LidarPropMinRange, 0.08);

ret = laser.initialize();
if ret:
Expand Down
2 changes: 1 addition & 1 deletion setup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ def build_extension(self, ext):

setup(
name='ydlidar',
version='1.0.0',
version='1.0.2',
author='Tony',
author_email='chushuifurong618@eaibot.com',
url='https://github.com/YDLIDAR/YDLidar-SDK',
Expand Down
26 changes: 23 additions & 3 deletions src/CYdLidar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,7 @@ bool CYdLidar::setlidaropt(int optname, const void *optval, int optlen) {
m_IgnoreString = (const char *)optval;
m_IgnoreArray = ydlidar::split(m_IgnoreString, ',');

if (m_IgnoreArray.size() / 2 != 0) {
if (m_IgnoreArray.size() % 2 != 0) {
m_IgnoreArray.clear();
ret = false;
}
Expand Down Expand Up @@ -430,7 +430,8 @@ bool CYdLidar::turnOn() {
if (checkLidarAbnormal()) {
lidarPtr->stop();
fprintf(stderr,
"[CYdLidar] Failed to turn on the Lidar, because the lidar is blocked or the lidar hardware is faulty.\n");
"[CYdLidar] Failed to turn on the Lidar, because the lidar is [%s].\n",
DriverInterface::DescribeDriverError(lidarPtr->getDriverError()));
isScanning = false;
return false;
}
Expand Down Expand Up @@ -565,7 +566,6 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) {
float intensity = 0.0;
float angle = 0.0;
debug.MaxDebugIndex = 0;
std::vector<LaserPoint> points;

for (int i = 0; i < count; i++) {
if (isNetTOFLidar(m_LidarType)) {
Expand Down Expand Up @@ -658,6 +658,12 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) {
// Error? Retry connection
}

if (lidarPtr->getDriverError() != NoError) {
fprintf(stderr, "[YDLIDAR ERROR]: %s\n",
DriverInterface::DescribeDriverError(lidarPtr->getDriverError()));
fflush(stderr);
}

m_AllNode = 0;
m_FristNodeTime = tim_scan_start;
}
Expand Down Expand Up @@ -723,6 +729,18 @@ const char *CYdLidar::DescribeError() const {
return value;
}

/*-------------------------------------------------------------
getDriverError
-------------------------------------------------------------*/
DriverError CYdLidar::getDriverError() const {
DriverError er = UnknownError;

if (lidarPtr) {
return lidarPtr->getDriverError();
}

return er;
}

/*-------------------------------------------------------------
isRangeValid
Expand Down Expand Up @@ -865,6 +883,8 @@ bool CYdLidar::checkLidarAbnormal() {
return !IS_OK(op_result);
}
}
} else {
check_abnormal_count++;
}
}

Expand Down
6 changes: 6 additions & 0 deletions src/CYdLidar.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -911,6 +911,12 @@ class YDLIDAR_API CYdLidar {
*/
const char *DescribeError() const;

/**
* @brief getDriverError
* @return
*/
DriverError getDriverError() const;

private:
/**
* @brief check LiDAR instance and connect to LiDAR,
Expand Down
79 changes: 77 additions & 2 deletions src/ETLidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,7 @@ result_t ETLidarDriver::connect(const char *port_path, uint32_t baudrate) {
m_isConnected = false;

if (!configPortConnect(serial_port.c_str(), m_port)) {
setDriverError(NotOpenError);
m_isValidDevice = false;
return RESULT_FAIL;
}
Expand Down Expand Up @@ -155,6 +156,7 @@ result_t ETLidarDriver::connect(const char *port_path, uint32_t baudrate) {

if (!dataPortConnect(m_config.deviceIp, m_config.dataRecvPort)) {
stopMeasure();
setDriverError(NotOpenError);
return RESULT_FAIL;
}

Expand Down Expand Up @@ -896,6 +898,8 @@ result_t ETLidarDriver::startAutoScan(bool force, uint32_t timeout) {
result_t ETLidarDriver::checkAutoConnecting() {
result_t ans = RESULT_FAIL;
isAutoconnting = true;
m_InvalidNodeCount = 0;
setDriverError(TimeoutError);

while (isAutoReconnect && isAutoconnting) {
{
Expand All @@ -906,6 +910,21 @@ result_t ETLidarDriver::checkAutoConnecting() {
return RESULT_FAIL;
}

if (socket_data->isOpen()) {
size_t buffer_size = socket_data->available();

if (m_BufferSize && m_BufferSize % 90 == 0) {
setDriverError(BlockError);
} else {
if (buffer_size > 0 || m_BufferSize > 0) {
setDriverError(TrembleError);
m_BufferSize += buffer_size;
} else {
setDriverError(NotBufferError);
}
}
}

socket_data->Close();
}
retryCount++;
Expand All @@ -914,7 +933,14 @@ result_t ETLidarDriver::checkAutoConnecting() {
retryCount = 100;
}

delay(100 * retryCount);
int tempCount = 0;

while (isAutoReconnect && isscanning() && tempCount < retryCount) {
delay(100);
tempCount++;
}

tempCount = 0;
int retryConnect = 0;

while (isAutoReconnect &&
Expand All @@ -925,7 +951,12 @@ result_t ETLidarDriver::checkAutoConnecting() {
retryConnect = 25;
}

delay(200 * retryConnect);
setDriverError(NotOpenError);

while (isAutoReconnect && isscanning() && tempCount < retryConnect) {
delay(200);
tempCount++;
}
}

if (!isAutoReconnect) {
Expand All @@ -944,8 +975,14 @@ result_t ETLidarDriver::checkAutoConnecting() {
}

if (IS_OK(ans)) {
if (getDriverError() == DeviceNotFoundError) {
setDriverError(NoError);
}

isAutoconnting = false;
return ans;
} else {
setDriverError(DeviceNotFoundError);
}
}
}
Expand All @@ -954,6 +991,20 @@ result_t ETLidarDriver::checkAutoConnecting() {

}

void ETLidarDriver::CheckLaserStatus() {
if (m_InvalidNodeCount < 2) {
if (m_driverErrno == NoError) {
setDriverError(LaserFailureError);
}
} else {
if (m_driverErrno == LaserFailureError) {
setDriverError(NoError);
}
}

m_InvalidNodeCount = 0;
}

int ETLidarDriver::cacheScanData() {
node_info local_buf[100];
size_t count = 100;
Expand All @@ -965,6 +1016,9 @@ int ETLidarDriver::cacheScanData() {

int timeout_count = 0;
retryCount = 0;
m_BufferSize = 0;
m_InvalidNodeCount = 0;
bool m_last_frame_valid = false;

while (m_isScanning) {
count = 100;
Expand All @@ -980,6 +1034,11 @@ int ETLidarDriver::cacheScanData() {
}
return RESULT_FAIL;
} else {
if (m_last_frame_valid) {
m_BufferSize = 0;
m_last_frame_valid = false;
}

ans = checkAutoConnecting();

if (IS_OK(ans)) {
Expand All @@ -993,12 +1052,23 @@ int ETLidarDriver::cacheScanData() {
} else {
timeout_count++;
local_scan[0].sync_flag = Node_NotSync;

if (m_driverErrno == NoError) {
setDriverError(TimeoutError);
}

fprintf(stderr, "timout count: %d\n", timeout_count);
fflush(stderr);
}
} else {
timeout_count = 0;
retryCount = 0;
m_BufferSize = 0;
m_last_frame_valid = true;

if (retryCount != 0) {
setDriverError(NoError);
}
}


Expand Down Expand Up @@ -1056,6 +1126,7 @@ result_t ETLidarDriver::waitScanData(node_info *nodebuffer, size_t &count,

if (node.sync_flag & LIDAR_RESP_MEASUREMENT_SYNCBIT) {
count = recvNodeCount;
CheckLaserStatus();
return RESULT_OK;
}

Expand Down Expand Up @@ -1090,6 +1161,10 @@ result_t ETLidarDriver::waitPackage(node_info *node, uint32_t timeout) {
(*node).distance_q2 = static_cast<uint16_t>(DSL(frame.frameBuf[offset + 2],
8) | DSL(frame.frameBuf[offset + 3], 0));

if ((*node).distance_q2 > 0) {
m_InvalidNodeCount++;
}

if (isV1Protocol(frame.dataFormat)) {
(*node).sync_quality = (uint16_t)(DSL(frame.frameBuf[offset],
8) | DSL(frame.frameBuf[offset + 1], 0));
Expand Down
5 changes: 5 additions & 0 deletions src/ETLidarDriver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -398,6 +398,11 @@ class ETLidarDriver : public DriverInterface {
*/
result_t checkAutoConnecting();

/**
* @brief CheckLaserStatus
*/
void CheckLaserStatus();

/**
* @brief parsing scan \n
*/
Expand Down
Loading

0 comments on commit 6dc2502

Please sign in to comment.