From d275bf43f5ceecd668b17d4fb0a77ab6b9262459 Mon Sep 17 00:00:00 2001 From: DavidHu Date: Thu, 20 Oct 2022 22:11:23 +0800 Subject: [PATCH] =?UTF-8?q?[update]=E7=89=88=E6=9C=AC=E5=8F=B7v3.0.2?= =?UTF-8?q?=EF=BC=8C=E5=AE=8C=E6=88=90SDK=E5=AF=B9=E5=A4=96=E7=9A=84?= =?UTF-8?q?=E6=8E=A5=E5=8F=A3Start(=20)=E5=86=85=E9=83=A8=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E5=AF=B9=E6=95=B0=E6=8D=AE=E5=A4=84=E7=90=86=E7=B1=BB?= =?UTF-8?q?=E7=8A=B6=E6=80=81=E6=A0=87=E5=BF=97=E5=8F=98=E9=87=8F=E8=BF=9B?= =?UTF-8?q?=E8=A1=8C=E5=A4=8D=E4=BD=8D=E6=93=8D=E4=BD=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ldlidar_driver/include/core/ldlidar_driver.h | 10 ++-- ldlidar_driver/include/dataprocess/lipkg.h | 23 ++++++--- ldlidar_driver/include/filter/tofbf.h | 11 ++--- ldlidar_driver/src/core/ldlidar_driver.cpp | 26 ++++++----- ldlidar_driver/src/dataprocess/lipkg.cpp | 49 ++++++++++---------- src/publish_node/main.cpp | 10 ++-- 6 files changed, 69 insertions(+), 60 deletions(-) diff --git a/ldlidar_driver/include/core/ldlidar_driver.h b/ldlidar_driver/include/core/ldlidar_driver.h index e044286..a0a22a8 100644 --- a/ldlidar_driver/include/core/ldlidar_driver.h +++ b/ldlidar_driver/include/core/ldlidar_driver.h @@ -41,7 +41,7 @@ typedef enum CommunicationMode { }CommunicationModeTypeDef; class LDLidarDriver { -public: + public: LDLidarDriver(); ~LDLidarDriver(); @@ -89,10 +89,10 @@ class LDLidarDriver { * @brief Whether the connection of the communication channel is normal after the lidar is powered on * @param[in] * *@param timeout: Wait timeout, in milliseconds - * @retval if times >= 1000, return false, communication connection is fail; - * if "times < 1000", return ture, communication connection is successful. + * @retval if times >= timeout, return false, communication connection is fail; + * if "times < timeout", return ture, communication connection is successful. */ - bool WaitLidarCommConnect(int64_t timeout = 1000); + bool WaitLidarCommConnect(int64_t timeout); /** * @brief get lidar laser scan point cloud data @@ -146,7 +146,7 @@ class LDLidarDriver { static void SetIsOkStatus(bool status) { is_ok_ = status;} -private: + private: std::string sdk_version_number_; static bool is_ok_; bool is_start_flag_; diff --git a/ldlidar_driver/include/dataprocess/lipkg.h b/ldlidar_driver/include/dataprocess/lipkg.h index 55f85a3..927b02e 100644 --- a/ldlidar_driver/include/dataprocess/lipkg.h +++ b/ldlidar_driver/include/dataprocess/lipkg.h @@ -76,8 +76,6 @@ class LiPkg { bool GetLaserScanData(Points2D& out); - Points2D GetLaserScanData(void); - void RegisterTimestampGetFunctional(std::function timestamp_handle); bool GetLidarPowerOnCommStatus(void); @@ -86,11 +84,15 @@ class LiPkg { LidarStatus GetLidarStatus(void); - // Get lidar data frame ready flag - bool IsFrameReady(void); - // Lidar data frame readiness flag reset - void ResetFrameReady(void); - + void ClearDataProcessStatus(void) { + is_frame_ready_ = false; + is_poweron_comm_normal_ = false; + lidarstatus_ = LidarStatus::NORMAL; + last_pkg_timestamp_ = 0; + first_frame_ = true; + } + + private: LDType product_type_; uint16_t timestamp_; @@ -101,6 +103,8 @@ class LiPkg { LidarStatus lidarstatus_; int measure_point_frequence_; std::function get_timestamp_; + uint64_t last_pkg_timestamp_; + bool first_frame_; LiDARFrameTypeDef pkg_; Points2D frame_tmp_; @@ -116,6 +120,11 @@ class LiPkg { bool AssemblePacket(); void SetFrameReady(void); void SetLaserScanData(Points2D& src); + + // Get lidar data frame ready flag + bool IsFrameReady(void); + // Lidar data frame readiness flag reset + void ResetFrameReady(void); }; } // namespace ldlidar diff --git a/ldlidar_driver/include/filter/tofbf.h b/ldlidar_driver/include/filter/tofbf.h index 2ac591c..e29e927 100644 --- a/ldlidar_driver/include/filter/tofbf.h +++ b/ldlidar_driver/include/filter/tofbf.h @@ -41,6 +41,11 @@ enum class FilterType{ }; class Tofbf { + public: + Tofbf(int speed, LDType type); + ~Tofbf(); + std::vector Filter(const std::vector &tmp) const; + private: FilterType filter_type_; // Low intensity threshold @@ -55,12 +60,6 @@ class Tofbf { Tofbf &operator=(const Tofbf &) = delete; std::vector NearFilter(const std::vector &tmp) const; std::vector NoiseFilter(const std::vector &tmp) const; - - public: - Tofbf(int speed, LDType type); - std::vector Filter(const std::vector &tmp) const; - - ~Tofbf(); }; } // namespace ldlidar diff --git a/ldlidar_driver/src/core/ldlidar_driver.cpp b/ldlidar_driver/src/core/ldlidar_driver.cpp index b77bde8..04b5426 100644 --- a/ldlidar_driver/src/core/ldlidar_driver.cpp +++ b/ldlidar_driver/src/core/ldlidar_driver.cpp @@ -24,14 +24,15 @@ namespace ldlidar { bool LDLidarDriver::is_ok_ = false; -LDLidarDriver::LDLidarDriver() : sdk_version_number_("v3.0.1"), - is_start_flag_(false), - register_get_timestamp_handle_(nullptr), - comm_pkg_(new LiPkg()), - comm_serial_(new SerialInterfaceLinux()), - comm_tcp_network_(new TCPSocketInterfaceLinux()), - comm_udp_network_(new UDPSocketInterfaceLinux()) { - last_pubdata_times_ = std::chrono::steady_clock::now(); +LDLidarDriver::LDLidarDriver() + : sdk_version_number_("v3.0.2"), + is_start_flag_(false), + register_get_timestamp_handle_(nullptr), + comm_pkg_(new LiPkg()), + comm_serial_(new SerialInterfaceLinux()), + comm_tcp_network_(new TCPSocketInterfaceLinux()), + comm_udp_network_(new UDPSocketInterfaceLinux()) { + last_pubdata_times_ = std::chrono::steady_clock::now(); } LDLidarDriver::~LDLidarDriver() { @@ -78,6 +79,8 @@ bool LDLidarDriver::Start(LDType product_name, LD_LOG_ERROR("get timestamp fuctional is not register.",""); return false; } + + comm_pkg_->ClearDataProcessStatus(); comm_pkg_->RegisterTimestampGetFunctional(register_get_timestamp_handle_); comm_pkg_->SetProductType(product_name); @@ -128,6 +131,7 @@ bool LDLidarDriver::Start(LDType product_name, return false; } + comm_pkg_->ClearDataProcessStatus(); comm_pkg_->RegisterTimestampGetFunctional(register_get_timestamp_handle_); comm_pkg_->SetProductType(product_name); @@ -217,13 +221,13 @@ bool LDLidarDriver::WaitLidarCommConnect(int64_t timeout) { auto last_time = std::chrono::steady_clock::now(); bool is_recvflag = false; - while (!is_recvflag && \ - (std::chrono::duration_cast(std::chrono::steady_clock::now() - last_time).count() < timeout)) { + do { if (comm_pkg_->GetLidarPowerOnCommStatus()) { is_recvflag = true; } usleep(1000); - } + } while (!is_recvflag && (std::chrono::duration_cast( + std::chrono::steady_clock::now() - last_time).count() < timeout)); if (is_recvflag) { last_pubdata_times_ = std::chrono::steady_clock::now(); diff --git a/ldlidar_driver/src/dataprocess/lipkg.cpp b/ldlidar_driver/src/dataprocess/lipkg.cpp index aa3c213..18211ca 100644 --- a/ldlidar_driver/src/dataprocess/lipkg.cpp +++ b/ldlidar_driver/src/dataprocess/lipkg.cpp @@ -56,15 +56,18 @@ uint8_t CalCRC8(const uint8_t *data, uint16_t data_len) { return crc; } -LiPkg::LiPkg() : product_type_(LDType::NO_VERSION), - timestamp_(0), - speed_(0), - is_frame_ready_(false), - is_poweron_comm_normal_(false), - is_filter_(true), - lidarstatus_(LidarStatus::NORMAL), - measure_point_frequence_(4500), - get_timestamp_(nullptr){ +LiPkg::LiPkg() + : product_type_(LDType::NO_VERSION), + timestamp_(0), + speed_(0), + is_frame_ready_(false), + is_poweron_comm_normal_(false), + is_filter_(true), + lidarstatus_(LidarStatus::NORMAL), + measure_point_frequence_(4500), + get_timestamp_(nullptr), + last_pkg_timestamp_(0), + first_frame_(true) { } @@ -143,20 +146,19 @@ bool LiPkg::AnalysisOne(uint8_t byte) { bool LiPkg::Parse(const uint8_t *data, long len) { for (int i = 0; i < len; i++) { if (AnalysisOne(data[i])) { - static uint64_t last_pkg_timestamp = 0; is_poweron_comm_normal_ = true; // parse a package is success double diff = (pkg_.end_angle / 100 - pkg_.start_angle / 100 + 360) % 360; if (diff <= ((double)pkg_.speed * POINT_PER_PACK / measure_point_frequence_ * 1.5)) { - if (0 == last_pkg_timestamp) { - last_pkg_timestamp = get_timestamp_(); + if (0 == last_pkg_timestamp_) { + last_pkg_timestamp_ = get_timestamp_(); continue; } uint64_t current_pack_stamp = get_timestamp_(); int pkg_point_number = POINT_PER_PACK; double pack_stamp_point_step = - static_cast(current_pack_stamp - last_pkg_timestamp) / static_cast(pkg_point_number - 1); + static_cast(current_pack_stamp - last_pkg_timestamp_) / static_cast(pkg_point_number - 1); speed_ = pkg_.speed; // Degrees per second timestamp_ = pkg_.timestamp; // In milliseconds @@ -171,11 +173,11 @@ bool LiPkg::Parse(const uint8_t *data, long len) { data.angle -= 360.0; } data.intensity = pkg_.point[i].intensity; - data.stamp = static_cast(last_pkg_timestamp + (pack_stamp_point_step * i)); + data.stamp = static_cast(last_pkg_timestamp_ + (pack_stamp_point_step * i)); frame_tmp_.push_back(PointData(data.angle, data.distance, data.intensity, data.stamp)); } - last_pkg_timestamp = current_pack_stamp; //// update last pkg timestamp + last_pkg_timestamp_ = current_pack_stamp; //// update last pkg timestamp } } } @@ -213,9 +215,8 @@ bool LiPkg::AssemblePacket() { std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b) { return a.stamp < b.stamp; }); if (tmp.size() > 0) { - static bool first_frame = true; - if (first_frame) { - first_frame = false; + if (first_frame_) { + first_frame_ = false; } else { SetLaserScanData(tmp); SetFrameReady(); @@ -290,11 +291,6 @@ bool LiPkg::GetLaserScanData(Points2D& out) { } } -Points2D LiPkg::GetLaserScanData(void) { - std::lock_guard lg(mutex_lock2_); - return laser_scan_data_; -} - void LiPkg::SetLaserScanData(Points2D& src) { std::lock_guard lg(mutex_lock2_); laser_scan_data_ = src; @@ -305,7 +301,12 @@ void LiPkg::RegisterTimestampGetFunctional(std::function timesta } bool LiPkg::GetLidarPowerOnCommStatus(void) { - return is_poweron_comm_normal_; + if (is_poweron_comm_normal_) { + is_poweron_comm_normal_ = false; + return true; + } else { + return false; + } } void LiPkg::EnableFilter(bool is_enable) { diff --git a/src/publish_node/main.cpp b/src/publish_node/main.cpp index f972e5b..823a479 100644 --- a/src/publish_node/main.cpp +++ b/src/publish_node/main.cpp @@ -81,7 +81,7 @@ int main(int argc, char **argv) { exit(EXIT_FAILURE); } - if (ldlidarnode->WaitLidarCommConnect(500)) { + if (ldlidarnode->WaitLidarCommConnect(3000)) { ROS_INFO("ldlidar communication is normal."); } else { ROS_ERROR("ldlidar communication is abnormal."); @@ -95,16 +95,12 @@ int main(int argc, char **argv) { ros::Rate r(10); //10hz ldlidar::Points2D laser_scan_points; double lidar_spin_freq; - bool is_get = false; + ROS_INFO("Publish topic message:ldlidar scan data ."); while (ros::ok()) { - switch (ldlidarnode->GetLaserScanData(laser_scan_points, 1000)){ + switch (ldlidarnode->GetLaserScanData(laser_scan_points, 1500)){ case ldlidar::LidarStatus::NORMAL: - if (!is_get) { - is_get = true; - ROS_INFO("get ldlidar normal data and publish topic message."); - } ldlidarnode->GetLidarSpinFreq(lidar_spin_freq); ToLaserscanMessagePublish(laser_scan_points, lidar_spin_freq, setting, lidar_pub); break;