Skip to content

Commit

Permalink
[update]版本号v3.0.3,增加SBC平台测试代码;删除不成熟的堵转和遮挡判断逻辑
Browse files Browse the repository at this point in the history
  • Loading branch information
ldrobotsensor committed Oct 26, 2022
1 parent 224fefa commit d746d26
Show file tree
Hide file tree
Showing 10 changed files with 365 additions and 76 deletions.
3 changes: 2 additions & 1 deletion .gitignore
Original file line number Diff line number Diff line change
@@ -1,2 +1,3 @@
*.json
build/**
build/**
src/test_sys/build/**
2 changes: 1 addition & 1 deletion ldlidar_driver/include/core/ldlidar_driver.h
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ class LDLidarDriver {
*/
LidarStatus GetLaserScanData(Points2D& dst, int64_t timeout = 1000);

bool GetLidarSpinFreq(double& spin_hz);
bool GetLidarScanFreq(double& spin_hz);

/**
* @brief register get timestamp handle functional.
Expand Down
4 changes: 2 additions & 2 deletions ldlidar_driver/include/dataprocess/lipkg.h
Original file line number Diff line number Diff line change
Expand Up @@ -134,9 +134,9 @@ class LiPkg {

Points2D GetLaserScanData(void);

void AnalysisLidarIsBlocking(uint16_t lidar_speed_val);
// void AnalysisLidarIsBlocking(uint16_t lidar_speed_val);

void AnalysisLidarIsOcclusion(Points2D& lidar_data);
// void AnalysisLidarIsOcclusion(Points2D& lidar_data);
};

} // namespace ldlidar
Expand Down
4 changes: 2 additions & 2 deletions ldlidar_driver/src/core/ldlidar_driver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ namespace ldlidar {

bool LDLidarDriver::is_ok_ = false;

LDLidarDriver::LDLidarDriver() : sdk_pack_version_("3.0.2"),
LDLidarDriver::LDLidarDriver() : sdk_pack_version_("3.0.3"),
is_start_flag_(false),
comm_pkg_(new LiPkg()),
comm_serial_(new SerialInterfaceLinux()){
Expand Down Expand Up @@ -142,7 +142,7 @@ LidarStatus LDLidarDriver::GetLaserScanData(Points2D& dst, int64_t timeout) {
}
}

bool LDLidarDriver::GetLidarSpinFreq(double& spin_hz) {
bool LDLidarDriver::GetLidarScanFreq(double& spin_hz) {
if (!is_start_flag_) {
return false;
}
Expand Down
95 changes: 48 additions & 47 deletions ldlidar_driver/src/dataprocess/lipkg.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -146,11 +146,10 @@ bool LiPkg::Parse(const uint8_t *data, long len) {

speed_ = datapkg_.speed;
timestamp_ = datapkg_.timestamp;
// judge block
AnalysisLidarIsBlocking(datapkg_.speed);

// parse a package is success
double diff = (datapkg_.end_angle / 100 - datapkg_.start_angle / 100 + 360) % 360;
if (diff <= ((double)datapkg_.speed * POINT_PER_PACK / 2300 * 1.5)) {
if (diff <= ((double)datapkg_.speed * POINT_PER_PACK / kPointFrequence * 1.5)) {

if (0 == last_pkg_timestamp_) {
last_pkg_timestamp_ = get_timestamp_();
Expand Down Expand Up @@ -221,7 +220,7 @@ bool LiPkg::AssemblePacket() {
std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b) { return a.stamp < b.stamp; });
if (tmp.size() > 0) {
SetLaserScanData(tmp);
AnalysisLidarIsOcclusion(tmp);


if (first_flag_) {
first_flag_ = false;
Expand Down Expand Up @@ -324,49 +323,51 @@ Points2D LiPkg::GetLaserScanData(void) {
return lidar_frame_data_;
}

void LiPkg::AnalysisLidarIsBlocking(uint16_t lidar_speed_val){
static int16_t judge_block_cnt = 0;
static uint16_t last_speed = 0;
uint16_t curr_speed = lidar_speed_val;
if ((curr_speed == 0) && (last_speed == 0)) {
judge_block_cnt++;
} else {
judge_block_cnt--;
if (judge_block_cnt <= 0) {
judge_block_cnt = 0;
}
}
if (judge_block_cnt >= 5) {
SetLidarStatus(LidarStatus::ERROR);
SetLidarErrorCode(LIDAR_ERROR_BLOCKING);
} else {
SetLidarStatus(LidarStatus::NORMAL);
SetLidarErrorCode(LIDAR_NO_ERROR);
}
last_speed = curr_speed;
}

void LiPkg::AnalysisLidarIsOcclusion(Points2D& lidar_data) {

uint16_t no_occlusion_count = 0;
for (auto point: lidar_data) {
if (point.distance != 0) {
no_occlusion_count++;
}
}

if (no_occlusion_count <= 5) {
SetLidarStatus(LidarStatus::ERROR);
if (GetLidarErrorCode() == LIDAR_ERROR_BLOCKING) {
SetLidarErrorCode(LIDAR_ERROR_BLOCKING_AND_OCCLUSION);
} else {
SetLidarErrorCode(LIDAR_ERROR_OCCLUSION);
}
} else {
SetLidarStatus(LidarStatus::NORMAL);
SetLidarErrorCode(LIDAR_NO_ERROR);
}
}
// BUG this function is unstable .
// void LiPkg::AnalysisLidarIsBlocking(uint16_t lidar_speed_val){
// static int16_t judge_block_cnt = 0;
// static uint16_t last_speed = 0;
// uint16_t curr_speed = lidar_speed_val;
// if ((curr_speed == 0) && (last_speed == 0)) {
// judge_block_cnt++;
// } else {
// judge_block_cnt--;
// if (judge_block_cnt <= 0) {
// judge_block_cnt = 0;
// }
// }
// if (judge_block_cnt >= 5) {
// SetLidarStatus(LidarStatus::ERROR);
// SetLidarErrorCode(LIDAR_ERROR_BLOCKING);
// } else {
// SetLidarStatus(LidarStatus::NORMAL);
// SetLidarErrorCode(LIDAR_NO_ERROR);
// }
// last_speed = curr_speed;
// }

// BUG this function is unstable .
// void LiPkg::AnalysisLidarIsOcclusion(Points2D& lidar_data) {

// uint16_t no_occlusion_count = 0;
// for (auto point: lidar_data) {
// if (point.distance != 0) {
// no_occlusion_count++;
// }
// }

// if (no_occlusion_count <= 5) {
// SetLidarStatus(LidarStatus::ERROR);
// if (GetLidarErrorCode() == LIDAR_ERROR_BLOCKING) {
// SetLidarErrorCode(LIDAR_ERROR_BLOCKING_AND_OCCLUSION);
// } else {
// SetLidarErrorCode(LIDAR_ERROR_OCCLUSION);
// }
// } else {
// SetLidarStatus(LidarStatus::NORMAL);
// SetLidarErrorCode(LIDAR_NO_ERROR);
// }
// }

} // namespace ldlidar

Expand Down
42 changes: 19 additions & 23 deletions src/linux_demo/demo.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,16 @@ uint64_t GetTimestamp(void) {
return ((uint64_t)tmp.count());
}

// void LidarPowerOn(void) {
// LDS_LOG_DEBUG("Lidar Power On","");
// // ...
// }

// void LidarPowerOff(void) {
// LDS_LOG_DEBUG("Lidar Power Off","");
// // ...
// }

int main(int argc, char **argv) {

if (argc != 2) {
Expand All @@ -38,7 +48,6 @@ int main(int argc, char **argv) {
LDS_LOG_INFO("or","");
LDS_LOG_INFO("./ldlidar_sl_node /dev/ttyS0","");
exit(EXIT_FAILURE);
exit(EXIT_FAILURE);
}

std::string port_name(argv[1]);
Expand All @@ -52,7 +61,8 @@ int main(int argc, char **argv) {
node->EnableFilterAlgorithnmProcess(true);

if (node->Start(ldlidar::LDType::LD_14, port_name)) {
LDS_LOG_INFO("ldldiar node start is success","");
LDS_LOG_INFO("ldlidar node start is success","");
// LidarPowerOn();
} else {
LDS_LOG_ERROR("ldlidar node start is fail","");
exit(EXIT_FAILURE);
Expand All @@ -62,23 +72,24 @@ int main(int argc, char **argv) {
LDS_LOG_INFO("ldlidar communication is normal.","");
} else {
LDS_LOG_ERROR("ldlidar communication is abnormal.","");
exit(EXIT_FAILURE);
node->Stop();
}

ldlidar::Points2D laser_scan_points;
while (ldlidar::LDLidarDriver::IsOk()) {

switch (node->GetLaserScanData(laser_scan_points, 1500)){
case ldlidar::LidarStatus::NORMAL: {
double lidar_spin_freq = 0;
node->GetLidarSpinFreq(lidar_spin_freq);
double lidar_scan_freq = 0;
node->GetLidarScanFreq(lidar_scan_freq);
#ifdef __LP64__
LDS_LOG_INFO("speed(Hz):%f, size:%d,stamp_front:%lu, stamp_back:%lu",
lidar_spin_freq, laser_scan_points.size(), laser_scan_points.front().stamp, laser_scan_points.back().stamp);
lidar_scan_freq, laser_scan_points.size(), laser_scan_points.front().stamp, laser_scan_points.back().stamp);
#else
LDS_LOG_INFO("speed(Hz):%f, size:%d,stamp_front:%llu, stamp_back:%llu",
lidar_spin_freq, laser_scan_points.size(), laser_scan_points.front().stamp, laser_scan_points.back().stamp);
lidar_scan_freq, laser_scan_points.size(), laser_scan_points.front().stamp, laser_scan_points.back().stamp);
#endif
// output 2d point cloud data
for (auto point : laser_scan_points) {
#ifdef __LP64__
LDS_LOG_INFO("stamp(ns):%lu,angle:%f,distance(mm):%d,intensity:%d",
Expand All @@ -90,19 +101,6 @@ int main(int argc, char **argv) {
}
break;
}
case ldlidar::LidarStatus::ERROR: {
uint8_t errcode = node->GetLidarErrorCode();
LDS_LOG_ERROR("ldlidar feedback errcode:%d",errcode);
if (LIDAR_ERROR_BLOCKING == errcode) {
LDS_LOG_WARN("ldlidar blocking","");
} else if (LIDAR_ERROR_OCCLUSION == errcode) {
LDS_LOG_WARN("ldlidar occlusion","");
} else if (LIDAR_ERROR_BLOCKING_AND_OCCLUSION == errcode) {
LDS_LOG_WARN("ldlidar blocking and occlusion","");
}
node->Stop();
break;
}
case ldlidar::LidarStatus::DATA_TIME_OUT: {
LDS_LOG_ERROR("ldlidar point cloud data publish time out, please check your lidar device.","");
node->Stop();
Expand All @@ -111,9 +109,6 @@ int main(int argc, char **argv) {
case ldlidar::LidarStatus::DATA_WAIT: {
break;
}
case ldlidar::LidarStatus::STOP: {
break;
}
default:
break;
}
Expand All @@ -122,6 +117,7 @@ int main(int argc, char **argv) {
}

node->Stop();
// LidarPowerOff();

delete node;
node = nullptr;
Expand Down
74 changes: 74 additions & 0 deletions src/test_sys/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
cmake_minimum_required(VERSION 2.8.3)
project(ldlidar_sl)

if(${CMAKE_BUILD_TYPE} MATCHES "Release")
#set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -std=c++11 -Wall")
#set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -std=c++11 -O3 -Wall")
message(STATUS "Mode: Release")
message(STATUS "optional:-std=c++11 -Wall")
else()
#set(CMAKE_BUILD_TYPE "Debug")
set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -std=c++11 -Wall -Wextra -Wpedantic -g2 -ggdb")
#set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -std=c++11 -O2 -Wall -Wextra -Wpedantic -g2 -ggdb")
message(STATUS "Mode: Debug")
message(STATUS "optional:-std=c++11 -Wall -Wextra -Wpedantic -g2 -ggdb")
endif()

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
${CMAKE_CURRENT_SOURCE_DIR}/../../ldlidar_driver/include/core/
${CMAKE_CURRENT_SOURCE_DIR}/../../ldlidar_driver/include/dataprocess/
${CMAKE_CURRENT_SOURCE_DIR}/../../ldlidar_driver/include/filter/
${CMAKE_CURRENT_SOURCE_DIR}/../../ldlidar_driver/include/logger/
${CMAKE_CURRENT_SOURCE_DIR}/../../ldlidar_driver/include/serialcom/
)

file(GLOB LDLIDAR_DRI_CORE ${CMAKE_CURRENT_SOURCE_DIR}/../../ldlidar_driver/src/core/*.cpp)
file(GLOB LDLIDAR_DRI_DATARPC ${CMAKE_CURRENT_SOURCE_DIR}/../../ldlidar_driver/src/dataprocess/*.cpp)
file(GLOB LDLIDAR_DRI_FILTER ${CMAKE_CURRENT_SOURCE_DIR}/../../ldlidar_driver/src/filter/*.cpp)
file(GLOB LDLIDAR_DRI_LOGGER ${CMAKE_CURRENT_SOURCE_DIR}/../../ldlidar_driver/src/logger/*.cpp)
file(GLOB LDLIDAR_DRI_SERIAL ${CMAKE_CURRENT_SOURCE_DIR}/../../ldlidar_driver/src/serialcom/*.cpp)

add_executable(${PROJECT_NAME}_node ${CMAKE_CURRENT_SOURCE_DIR}/test.cpp)

add_library(ldlidar_driver_static STATIC
${LDLIDAR_DRI_CORE}
${LDLIDAR_DRI_DATARPC}
${LDLIDAR_DRI_FILTER}
${LDLIDAR_DRI_LOGGER}
${LDLIDAR_DRI_SERIAL}
)

add_library(ldlidar_driver_shared SHARED
${LDLIDAR_DRI_CORE}
${LDLIDAR_DRI_DATARPC}
${LDLIDAR_DRI_FILTER}
${LDLIDAR_DRI_LOGGER}
${LDLIDAR_DRI_SERIAL}
)

# rename library name
set_target_properties (ldlidar_driver_static PROPERTIES OUTPUT_NAME "ldlidar_driver")
set_target_properties (ldlidar_driver_shared PROPERTIES OUTPUT_NAME "ldlidar_driver")

# binary file link to library
target_link_libraries(${PROJECT_NAME}_node ldlidar_driver_static pthread wiringPi)

###########
## Install ##
###########

INSTALL(TARGETS ldlidar_driver_static ldlidar_driver_shared
ARCHIVE DESTINATION lib/ldlidar_driver
LIBRARY DESTINATION share/ldlidar_driver
)

INSTALL(DIRECTORY ${PROJECT_SOURCE_DIR}/ldlidar_driver/include
DESTINATION include/ldlidar_driver
)
21 changes: 21 additions & 0 deletions src/test_sys/auto_build_test.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
#!/bin/bash
#Author: David Hu
#Date: 2022-09

# Exit on error
set -e

echo "Start cmake build"

if [ ! -e "./build" ];then
mkdir build
echo "create ./build/"
fi

cd ./build

cmake -DCMAKE_BUILD_TYPE=Debug ..

make -j2

echo "build finished."
14 changes: 14 additions & 0 deletions src/test_sys/clean_build.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
#!/bin/bash
#Author: David Hu
#Date: 2022-09
if [ -e "./build" ];then
rm -rf build/
echo "del ./build/"
fi

if [ -e "./install" ];then
rm -rf install/
echo "del ./install/"
fi

echo "del is ok....."
Loading

0 comments on commit d746d26

Please sign in to comment.