From 73cd4afb937c7f7f48be2d1ea8573982a76e4edd Mon Sep 17 00:00:00 2001 From: DavidHu Date: Sun, 29 May 2022 21:15:24 +0800 Subject: [PATCH] =?UTF-8?q?[update]=E7=89=88=E6=9C=AC=E5=8F=B7v2.3.3?= =?UTF-8?q?=EF=BC=8C=E6=9B=B4=E6=94=B9=E7=82=B9=E4=B8=BA:=201.=20=E4=BF=AE?= =?UTF-8?q?=E6=94=B9=E8=BE=93=E5=87=BASDK=E7=89=88=E6=9C=AC=E5=8F=B7?= =?UTF-8?q?=E7=9A=84=E6=96=B9=E5=BC=8F;=202.=20=E6=B7=BB=E5=8A=A0=E5=90=8D?= =?UTF-8?q?=E7=A9=BA=E9=97=B4=E3=80=81=E4=BF=AE=E6=94=B9=E5=A4=B4=E6=96=87?= =?UTF-8?q?=E4=BB=B6=E5=8C=85=E5=90=AB=E4=BD=BF=E6=9B=B4=E5=85=B7=E7=A7=BB?= =?UTF-8?q?=E6=A4=8D=E6=80=A7;=203.=20=E4=BF=AE=E6=94=B9=E8=84=9A=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ldlidar_driver/cmd_interface_linux.cpp | 15 ++++-------- .../include/cmd_interface_linux.h | 11 +++++++++ include/ldlidar_driver/include/lipkg.h | 19 ++++++++------- include/ldlidar_driver/include/pointdata.h | 8 +++++-- include/ldlidar_driver/include/tofbf.h | 8 +++++-- include/ldlidar_driver/lipkg.cpp | 19 +++++++-------- include/ldlidar_driver/tofbf.cpp | 6 ++--- src/publish_node/main.cpp | 23 +++++++++++-------- ws_deploy.sh | 3 ++- 9 files changed, 63 insertions(+), 49 deletions(-) diff --git a/include/ldlidar_driver/cmd_interface_linux.cpp b/include/ldlidar_driver/cmd_interface_linux.cpp index 402dc39..db868b0 100644 --- a/include/ldlidar_driver/cmd_interface_linux.cpp +++ b/include/ldlidar_driver/cmd_interface_linux.cpp @@ -19,15 +19,7 @@ #include "cmd_interface_linux.h" -#include -#include -#include -#include -#include -#include -#include - -#include +namespace ldlidar { #define MAX_ACK_BUF_LEN 4096 @@ -55,8 +47,8 @@ bool CmdInterfaceLinux::Open(std::string &port_name) { return false; } - options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8 | CRTSCTS); - options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB | PARODD); + options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8); + options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB); options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN); //|ECHOPRT options.c_oflag &= (tcflag_t) ~(OPOST); @@ -166,5 +158,6 @@ void CmdInterfaceLinux::RxThreadProc(void *param) { delete[] rx_buf; } +} // namespace ldlidar /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF * FILE ********/ \ No newline at end of file diff --git a/include/ldlidar_driver/include/cmd_interface_linux.h b/include/ldlidar_driver/include/cmd_interface_linux.h index 5e26d25..7bca7c9 100644 --- a/include/ldlidar_driver/include/cmd_interface_linux.h +++ b/include/ldlidar_driver/include/cmd_interface_linux.h @@ -21,8 +21,15 @@ #define __LINUX_SERIAL_PORT_H__ #include +#include +#include +#include #include +#include +#include +#include +#include #include #include #include @@ -31,6 +38,8 @@ #include #include +namespace ldlidar { + class CmdInterfaceLinux { public: CmdInterfaceLinux(); @@ -59,6 +68,8 @@ class CmdInterfaceLinux { static void RxThreadProc(void *param); }; +} // namespace ldlidar + #endif //__LINUX_SERIAL_PORT_H__ /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF diff --git a/include/ldlidar_driver/include/lipkg.h b/include/ldlidar_driver/include/lipkg.h index bba4b0d..074fe19 100644 --- a/include/ldlidar_driver/include/lipkg.h +++ b/include/ldlidar_driver/include/lipkg.h @@ -22,19 +22,13 @@ #ifndef __LIPKG_H #define __LIPKG_H -// #include "ros_api.h" -#include -#include - -#include -#include -#include -#include -#include #include #include "cmd_interface_linux.h" #include "pointdata.h" +#include "tofbf.h" + +namespace ldlidar { enum { PKG_HEADER = 0x54, @@ -64,6 +58,8 @@ class LiPkg { LiPkg(); ~LiPkg(); + // get sdk pack version Number + std::string GetSdkVersionNumber(); // get Lidar spin speed (Hz) double GetSpeed(void); // get lidar spind speed (degree per second) origin @@ -81,6 +77,7 @@ class LiPkg { Points2D GetLaserScanData(void); private: + std::string sdk_pack_version_; uint16_t timestamp_; double speed_; long error_times_; @@ -97,9 +94,11 @@ class LiPkg { bool Parse(const uint8_t* data, long len); // combine stantard data into data frames and calibrate bool AssemblePacket(); - void FillLaserScanData(Points2D& src); + void SetLaserScanData(Points2D& src); }; +} // namespace ldlidar + #endif //__LIPKG_H /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF diff --git a/include/ldlidar_driver/include/pointdata.h b/include/ldlidar_driver/include/pointdata.h index 15277da..a134822 100644 --- a/include/ldlidar_driver/include/pointdata.h +++ b/include/ldlidar_driver/include/pointdata.h @@ -26,6 +26,11 @@ #include #include +namespace ldlidar { + +#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) +#define RADIAN_TO_ANGLED(angle) ((angle)*180000 / 3141.59) + struct PointData { // Polar coordinate representation float angle; // Angle ranges from 0 to 359 degrees @@ -52,8 +57,7 @@ struct PointData { typedef std::vector Points2D; -#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) -#define RADIAN_TO_ANGLED(angle) ((angle)*180000 / 3141.59) +} // namespace ldlidar #endif // _POINT_DATA_H_ diff --git a/include/ldlidar_driver/include/tofbf.h b/include/ldlidar_driver/include/tofbf.h index eb22978..9d4d309 100644 --- a/include/ldlidar_driver/include/tofbf.h +++ b/include/ldlidar_driver/include/tofbf.h @@ -22,12 +22,14 @@ #ifndef __TOFBF_H_ #define __TOFBF_H_ -#include +#include -#include +#include #include "pointdata.h" +namespace ldlidar { + class Tofbf { private: const int kIntensityLow = 15; // Low intensity threshold @@ -45,6 +47,8 @@ class Tofbf { ~Tofbf(); }; +} // namespace ldlidar + #endif //__TOFBF_H_ /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF diff --git a/include/ldlidar_driver/lipkg.cpp b/include/ldlidar_driver/lipkg.cpp index e0f3129..c47db2f 100644 --- a/include/ldlidar_driver/lipkg.cpp +++ b/include/ldlidar_driver/lipkg.cpp @@ -21,12 +21,7 @@ #include "lipkg.h" -#include -#include - -#include - -#include "tofbf.h" +namespace ldlidar { static const uint8_t CrcTable[256] = { 0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3, 0xae, 0xf2, 0xbf, 0x68, 0x25, @@ -62,7 +57,8 @@ uint8_t CalCRC8(const uint8_t *data, uint16_t data_len) { } LiPkg::LiPkg() - : timestamp_(0), + : sdk_pack_version_("v2.3.3"), + timestamp_(0), speed_(0), error_times_(0), is_frame_ready_(false){ @@ -73,6 +69,10 @@ LiPkg::~LiPkg() { } +std::string LiPkg::GetSdkVersionNumber() { + return sdk_pack_version_; +} + bool LiPkg::AnalysisOne(uint8_t byte) { static enum { HEADER, @@ -170,7 +170,7 @@ bool LiPkg::AssemblePacket() { tmp = tofFilter.NearFilter(data); std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b) { return a.angle < b.angle; }); if (tmp.size() > 0) { - FillLaserScanData(tmp); + SetLaserScanData(tmp); SetFrameReady(); frame_tmp_.erase(frame_tmp_.begin(), frame_tmp_.begin() + count); return true; @@ -224,10 +224,11 @@ Points2D LiPkg::GetLaserScanData(void) { return laser_scan_data_; } -void LiPkg::FillLaserScanData(Points2D& src) { +void LiPkg::SetLaserScanData(Points2D& src) { std::lock_guard lg(mutex_lock2_); laser_scan_data_ = src; } +} // namespace ldlidar /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF * FILE ********/ \ No newline at end of file diff --git a/include/ldlidar_driver/tofbf.cpp b/include/ldlidar_driver/tofbf.cpp index e93d16a..07317b6 100644 --- a/include/ldlidar_driver/tofbf.cpp +++ b/include/ldlidar_driver/tofbf.cpp @@ -21,10 +21,7 @@ #include "tofbf.h" -#include - -#include -#include +namespace ldlidar { /** * @brief Construct a new Tofbf:: Tofbf object @@ -145,5 +142,6 @@ std::vector Tofbf::NearFilter( return normal; } +} // namespace ldlidar /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF * FILE ********/ \ No newline at end of file diff --git a/src/publish_node/main.cpp b/src/publish_node/main.cpp index d5f3154..caa9025 100644 --- a/src/publish_node/main.cpp +++ b/src/publish_node/main.cpp @@ -21,7 +21,7 @@ #include "ros_api.h" #include "lipkg.h" -void ToLaserscanMessagePublish(Points2D& src, LiPkg* commpkg, LaserScanSetting& setting, ros::Publisher& lidarpub); +void ToLaserscanMessagePublish(ldlidar::Points2D& src, ldlidar::LiPkg* commpkg, LaserScanSetting& setting, ros::Publisher& lidarpub); int main(int argc, char **argv) { ros::init(argc, argv, "ldldiar_publisher"); @@ -40,22 +40,23 @@ int main(int argc, char **argv) { nh_private.param("enable_angle_crop_func", setting.enable_angle_crop_func, bool(false)); nh_private.param("angle_crop_min", setting.angle_crop_min, double(0.0)); nh_private.param("angle_crop_max", setting.angle_crop_max, double(0.0)); - - ROS_INFO("[ldrobot] SDK Pack Version is v2.3.2"); + + ldlidar::LiPkg *lidar_commh = new ldlidar::LiPkg(); + ldlidar::CmdInterfaceLinux *cmd_port = new ldlidar::CmdInterfaceLinux(); + + ROS_INFO_STREAM("[ldrobot] SDK Pack Version is: " << lidar_commh->GetSdkVersionNumber()); ROS_INFO("[ldrobot] : %s,: %s,: %s,: %s", product_name.c_str(), topic_name.c_str(), port_name.c_str(), setting.frame_id.c_str()); ROS_INFO("[ldrobot] : %s,: %s,: %f,: %f", (setting.laser_scan_dir?"Counterclockwise":"Clockwise"), (setting.enable_angle_crop_func?"true":"false"), setting.angle_crop_min, setting.angle_crop_max); - LiPkg *lidar_commh = new LiPkg(); - CmdInterfaceLinux *cmd_port = new CmdInterfaceLinux(); - if (port_name.empty()) { ROS_ERROR("[ldrobot] input param is null"); exit(EXIT_FAILURE); } - cmd_port->SetReadCallback(std::bind(&LiPkg::CommReadCallback, lidar_commh, std::placeholders::_1, std::placeholders::_2)); + + cmd_port->SetReadCallback(std::bind(&ldlidar::LiPkg::CommReadCallback, lidar_commh, std::placeholders::_1, std::placeholders::_2)); if (cmd_port->Open(port_name)) { ROS_INFO("[ldrobot] open %s device %s is success", product_name.c_str(), port_name.c_str()); @@ -67,16 +68,18 @@ int main(int argc, char **argv) { ros::Publisher lidar_pub = nh.advertise(topic_name, 10); // create a ROS topic ros::Rate r(10); //10hz + auto last_time = std::chrono::steady_clock::now(); + while (ros::ok()) { if (lidar_commh->IsFrameReady()) { lidar_commh->ResetFrameReady(); last_time = std::chrono::steady_clock::now(); - Points2D laserscandata = lidar_commh->GetLaserScanData(); + ldlidar::Points2D laserscandata = lidar_commh->GetLaserScanData(); ToLaserscanMessagePublish(laserscandata, lidar_commh, setting, lidar_pub); } - if (std::chrono::duration_cast(std::chrono::steady_clock::now()-last_time).count() > 1000) { // 数据发布超时或者串口设备拔出处理 + if (std::chrono::duration_cast(std::chrono::steady_clock::now()-last_time).count() > 1000) { ROS_ERROR("[ldrobot] lidar pub data is time out, please check lidar device"); exit(EXIT_FAILURE); } @@ -93,7 +96,7 @@ int main(int argc, char **argv) { return 0; } -void ToLaserscanMessagePublish(Points2D& src, LiPkg* commpkg, LaserScanSetting& setting, ros::Publisher& lidarpub) { +void ToLaserscanMessagePublish(ldlidar::Points2D& src, ldlidar::LiPkg* commpkg, LaserScanSetting& setting, ros::Publisher& lidarpub) { float angle_min, angle_max, range_min, range_max, angle_increment; float scan_time; ros::Time start_scan_time; diff --git a/ws_deploy.sh b/ws_deploy.sh index a724c32..4f22a84 100644 --- a/ws_deploy.sh +++ b/ws_deploy.sh @@ -15,7 +15,8 @@ else cp ./launch ./${floder_name} -a cp ./rviz ./${floder_name} -a cp ./src ./${floder_name} -a - cp ./listen ./${floder_name} -a + cp ./include ./${floder_name} -a + cp ./scripts ./${floder_name} -a zip -r ${floder_name}.zip ./${floder_name} rm -rf ./${floder_name}/ echo "create ./${floder_name}.zip "