From 22942da7ec1d0a91ac8b02e4d73dc736de18c901 Mon Sep 17 00:00:00 2001 From: zhangyu Date: Wed, 30 Dec 2020 02:04:58 +0000 Subject: [PATCH 1/9] Optimizing computational efficiency --- .../src/pandarGeneral_internal.cc | 186 +++++++++++------- .../src/pandarGeneral_internal.h | 5 + 2 files changed, 122 insertions(+), 69 deletions(-) diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc index acd7b49..a5cf974 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc @@ -79,6 +79,7 @@ static const float pandar20_horizatal_azimuth_offset_map[] = { }; static std::vector > PointCloudList(128); +static std::vector PointCloud; PandarGeneral_Internal::PandarGeneral_Internal( std::string device_ip, uint16_t lidar_port, uint16_t gps_port, @@ -139,7 +140,12 @@ PandarGeneral_Internal::PandarGeneral_Internal(std::string pcap_path, \ connect_lidar_ = false; m_sTimestampType = timestampType; m_dPktTimestamp = 0.0f; - + sin_azimuth_map.resize(36000); + cos_azimuth_map.resize(36000); + for(int i = 0; i < 36000; ++i) { + sin_azimuth_map[i] = sinf(i * M_PI / 18000); + cos_azimuth_map[i] = cosf(i * M_PI / 18000); + } Init(); } @@ -436,7 +442,11 @@ void PandarGeneral_Internal::Init() { laserQTOffset_[63] = 10.0f + 136.45f; if (m_sLidarType == "Pandar40P" || m_sLidarType == "Pandar40M") { + sin_elevation_map.resize(LASER_COUNT); + cos_elevation_map.resize(LASER_COUNT); for (int i = 0; i < LASER_COUNT; i++) { + sin_elevation_map[i] = sinf(degreeToRadian(pandar40p_elev_angle_map[i])); + cos_elevation_map[i] = cosf(degreeToRadian(pandar40p_elev_angle_map[i])); General_elev_angle_map_[i] = pandar40p_elev_angle_map[i]; General_horizatal_azimuth_offset_map_[i] = \ pandar40p_horizatal_azimuth_offset_map[i]; @@ -444,7 +454,11 @@ void PandarGeneral_Internal::Init() { } if (m_sLidarType == "Pandar64") { + sin_elevation_map.resize(HS_LIDAR_L64_UNIT_NUM); + cos_elevation_map.resize(HS_LIDAR_L64_UNIT_NUM); for (int i = 0; i < HS_LIDAR_L64_UNIT_NUM; i++) { + sin_elevation_map[i] = sinf(degreeToRadian(pandarGeneral_elev_angle_map[i])); + cos_elevation_map[i] = cosf(degreeToRadian(pandarGeneral_elev_angle_map[i])); General_elev_angle_map_[i] = pandarGeneral_elev_angle_map[i]; General_horizatal_azimuth_offset_map_[i] = \ pandarGeneral_horizatal_azimuth_offset_map[i]; @@ -452,21 +466,33 @@ void PandarGeneral_Internal::Init() { } if (m_sLidarType == "Pandar20A" || m_sLidarType == "Pandar20B") { + sin_elevation_map.resize(HS_LIDAR_L20_UNIT_NUM); + cos_elevation_map.resize(HS_LIDAR_L20_UNIT_NUM); for (int i = 0; i < HS_LIDAR_L20_UNIT_NUM; i++) { + sin_elevation_map[i] = sinf(degreeToRadian(pandar20_elev_angle_map[i])); + cos_elevation_map[i] = cosf(degreeToRadian(pandar20_elev_angle_map[i])); General_elev_angle_map_[i] = pandar20_elev_angle_map[i]; General_horizatal_azimuth_offset_map_[i] = pandar20_horizatal_azimuth_offset_map[i]; } } if (m_sLidarType == "PandarQT") { + sin_elevation_map.resize(HS_LIDAR_QT_UNIT_NUM); + cos_elevation_map.resize(HS_LIDAR_QT_UNIT_NUM); for (int i = 0; i < HS_LIDAR_QT_UNIT_NUM; i++) { + sin_elevation_map[i] = sinf(degreeToRadian(pandarQT_elev_angle_map[i])); + cos_elevation_map[i] = cosf(degreeToRadian(pandarQT_elev_angle_map[i])); General_elev_angle_map_[i] = pandarQT_elev_angle_map[i]; General_horizatal_azimuth_offset_map_[i] = pandarQT_horizatal_azimuth_offset_map[i]; } } if (m_sLidarType == "PandarXT-32") { + sin_elevation_map.resize(HS_LIDAR_XT_UNIT_NUM); + cos_elevation_map.resize(HS_LIDAR_XT_UNIT_NUM); for (int i = 0; i < HS_LIDAR_XT_UNIT_NUM; i++) { + sin_elevation_map[i] = sinf(degreeToRadian(pandarXT_elev_angle_map[i])); + cos_elevation_map[i] = cosf(degreeToRadian(pandarXT_elev_angle_map[i])); General_elev_angle_map_[i] = pandarXT_elev_angle_map[i]; General_horizatal_azimuth_offset_map_[i] = pandarXT_horizatal_azimuth_offset_map[i]; laserXTOffset_[i] = laserXTOffset[i]; @@ -474,7 +500,11 @@ void PandarGeneral_Internal::Init() { } if (m_sLidarType == "PandarXT-16") { + sin_elevation_map.resize(HS_LIDAR_XT16_UNIT_NUM); + cos_elevation_map.resize(HS_LIDAR_XT16_UNIT_NUM); for (int i = 0; i < HS_LIDAR_XT16_UNIT_NUM; i++) { + sin_elevation_map[i] = sinf(degreeToRadian(pandarXT_elev_angle_map[i*2])); + cos_elevation_map[i] = cosf(degreeToRadian(pandarXT_elev_angle_map[i*2])); General_elev_angle_map_[i] = pandarXT_elev_angle_map[i*2]; General_horizatal_azimuth_offset_map_[i] = pandarXT_horizatal_azimuth_offset_map[i*2]; laserXTOffset_[i] = laserXTOffset[i*2]; @@ -531,10 +561,13 @@ int PandarGeneral_Internal::LoadCorrectionFile(std::string correction_content) { elev_angle[lineId - 1] = elev; azimuthOffset[lineId - 1] = azimuth; } - + sin_elevation_map.resize(lineCounter); + cos_elevation_map.resize(lineCounter); for (int i = 0; i < lineCounter; ++i) { /* for all the laser offset */ General_elev_angle_map_[i] = elev_angle[i]; + sin_elevation_map[i] = sinf(degreeToRadian(General_elev_angle_map_[i])); + cos_elevation_map[i] = cosf(degreeToRadian(General_elev_angle_map_[i])); General_horizatal_azimuth_offset_map_[i] = azimuthOffset[i]; } @@ -628,6 +661,8 @@ void PandarGeneral_Internal::ProcessLiarPacket() { int ret = 0; boost::shared_ptr outMsg(new PPointCloud()); + outMsg->points.resize(1); + PointCloud.reserve(70000); hesai_lidar::PandarScanPtr scan(new hesai_lidar::PandarScan); hesai_lidar::PandarPacket rawpacket; @@ -717,7 +752,9 @@ void PandarGeneral_Internal::ProcessLiarPacket() { if (pcl_callback_ && (outMsg->points.size() > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); - outMsg.reset(new PPointCloud()); + scan->packets.reserve(600); + PointCloud.clear(); + PointCloud.reserve(70000); } } } else { @@ -753,7 +790,9 @@ void PandarGeneral_Internal::ProcessLiarPacket() { if (pcl_callback_ && (outMsg->points.size() > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); - outMsg.reset(new PPointCloud()); + scan->packets.reserve(600); + PointCloud.clear(); + PointCloud.reserve(70000); } } } else { @@ -788,7 +827,9 @@ void PandarGeneral_Internal::ProcessLiarPacket() { if (pcl_callback_ && (outMsg->points.size() > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); - outMsg.reset(new PPointCloud()); + scan->packets.reserve(600); + PointCloud.clear(); + PointCloud.reserve(70000); } } } else { @@ -824,7 +865,9 @@ void PandarGeneral_Internal::ProcessLiarPacket() { if (pcl_callback_ && (outMsg->points.size() > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); - outMsg.reset(new PPointCloud()); + scan->packets.reserve(600); + PointCloud.clear(); + PointCloud.reserve(70000); } } } else { @@ -1267,17 +1310,15 @@ void PandarGeneral_Internal::CalcPointXYZIT(Pandar40PPacket *pkt, int blockid, continue; } - double xyDistance = unit.distance * \ - cosf(degreeToRadian(General_elev_angle_map_[i])); - - point.x = static_cast(xyDistance * \ - sinf(degreeToRadian(General_horizatal_azimuth_offset_map_[i] + \ - (static_cast(block->azimuth)) / 100.0))); - point.y = static_cast(xyDistance * \ - cosf(degreeToRadian(General_horizatal_azimuth_offset_map_[i] + \ - (static_cast(block->azimuth)) / 100.0))); - point.z = static_cast(unit.distance * \ - sinf(degreeToRadian(General_elev_angle_map_[i]))); + int azimuth = static_cast(General_horizatal_azimuth_offset_map_[i] * 100 + block->azimuth); + if(azimuth < 0) + azimuth += 36000; + if(azimuth > 36000) + azimuth -= 36000; + float xyDistance = unit.distance * cos_elevation_map[i]; + point.x = static_cast(xyDistance * sin_azimuth_map[azimuth]); + point.y = static_cast(xyDistance * cos_azimuth_map[azimuth]); + point.z = static_cast(unit.distance * sin_elevation_map[i]); point.intensity = unit.intensity; @@ -1305,7 +1346,7 @@ void PandarGeneral_Internal::CalcPointXYZIT(Pandar40PPacket *pkt, int blockid, if (pcl_type_) { PointCloudList[i].push_back(point); } else { - cld->push_back(point); + PointCloud.push_back(point); } } } @@ -1314,7 +1355,7 @@ void PandarGeneral_Internal::CalcL64PointXYZIT(HS_LIDAR_L64_Packet *pkt, int blo char chLaserNumber, boost::shared_ptr cld) { HS_LIDAR_L64_Block *block = &pkt->blocks[blockid]; - struct tm tTm; + struct tm tTm = {0}; // UTC's year only include 0 - 99 year , which indicate 2000 to 2099. // and mktime's year start from 1900 which is 0. so we need add 100 year. tTm.tm_year = pkt->addtime[0] + 100; @@ -1331,6 +1372,8 @@ void PandarGeneral_Internal::CalcL64PointXYZIT(HS_LIDAR_L64_Packet *pkt, int blo tTm.tm_min = pkt->addtime[4]; tTm.tm_sec = pkt->addtime[5]; tTm.tm_isdst = 0; + tTm.tm_gmtoff = 0; + tTm.tm_zone = 0; double unix_second = \ static_cast(mktime(&tTm) + tz_second_); @@ -1345,18 +1388,15 @@ void PandarGeneral_Internal::CalcL64PointXYZIT(HS_LIDAR_L64_Packet *pkt, int blo continue; } - double xyDistance = \ - unit.distance * cosf(degreeToRadian(General_elev_angle_map_[i])); - point.x = static_cast( - xyDistance * - sinf(degreeToRadian(General_horizatal_azimuth_offset_map_[i] + - (static_cast(block->azimuth)) / 100.0))); - point.y = static_cast( - xyDistance * - cosf(degreeToRadian(General_horizatal_azimuth_offset_map_[i] + - (static_cast(block->azimuth)) / 100.0))); - point.z = static_cast(unit.distance * - sinf(degreeToRadian(General_elev_angle_map_[i]))); + int azimuth = static_cast(General_horizatal_azimuth_offset_map_[i] * 100 + block->azimuth); + if(azimuth < 0) + azimuth += 36000; + if(azimuth > 36000) + azimuth -= 36000; + float xyDistance = unit.distance * cos_elevation_map[i]; + point.x = static_cast(xyDistance * sin_azimuth_map[azimuth]); + point.y = static_cast(xyDistance * cos_azimuth_map[azimuth]); + point.z = static_cast(unit.distance * sin_elevation_map[i]); point.intensity = unit.intensity; @@ -1385,7 +1425,7 @@ void PandarGeneral_Internal::CalcL64PointXYZIT(HS_LIDAR_L64_Packet *pkt, int blo if (pcl_type_) { PointCloudList[i].push_back(point); } else { - cld->push_back(point); + PointCloud.push_back(point); } } } @@ -1394,7 +1434,7 @@ void PandarGeneral_Internal::CalcL20PointXYZIT(HS_LIDAR_L20_Packet *pkt, int blo char chLaserNumber, boost::shared_ptr cld) { HS_LIDAR_L20_Block *block = &pkt->blocks[blockid]; - struct tm tTm; + struct tm tTm = {0}; // UTC's year only include 0 - 99 year , which indicate 2000 to 2099. // and mktime's year start from 1900 which is 0. so we need add 100 year. tTm.tm_year = pkt->addtime[0] + 100; @@ -1411,6 +1451,8 @@ void PandarGeneral_Internal::CalcL20PointXYZIT(HS_LIDAR_L20_Packet *pkt, int blo tTm.tm_min = pkt->addtime[4]; tTm.tm_sec = pkt->addtime[5]; tTm.tm_isdst = 0; + tTm.tm_gmtoff = 0; + tTm.tm_zone = 0; double unix_second = \ static_cast(mktime(&tTm) + tz_second_); @@ -1425,16 +1467,15 @@ void PandarGeneral_Internal::CalcL20PointXYZIT(HS_LIDAR_L20_Packet *pkt, int blo continue; } - double xyDistance = unit.distance * cosf(degreeToRadian(General_elev_angle_map_[i])); - - point.x = static_cast(xyDistance * \ - sinf(degreeToRadian(General_horizatal_azimuth_offset_map_[i] + \ - (static_cast(block->azimuth)) / 100.0))); - point.y = static_cast(xyDistance * \ - cosf(degreeToRadian(General_horizatal_azimuth_offset_map_[i] + \ - (static_cast(block->azimuth)) / 100.0))); - point.z = static_cast(unit.distance * \ - sinf(degreeToRadian(General_elev_angle_map_[i]))); + int azimuth = static_cast(General_horizatal_azimuth_offset_map_[i] * 100 + block->azimuth); + if(azimuth < 0) + azimuth += 36000; + if(azimuth > 36000) + azimuth -= 36000; + float xyDistance = unit.distance * cos_elevation_map[i]; + point.x = static_cast(xyDistance * sin_azimuth_map[azimuth]); + point.y = static_cast(xyDistance * cos_azimuth_map[azimuth]); + point.z = static_cast(unit.distance * sin_elevation_map[i]); point.intensity = unit.intensity; @@ -1474,7 +1515,7 @@ void PandarGeneral_Internal::CalcL20PointXYZIT(HS_LIDAR_L20_Packet *pkt, int blo if (pcl_type_) { PointCloudList[i].push_back(point); } else { - cld->push_back(point); + PointCloud.push_back(point); } } } @@ -1484,7 +1525,7 @@ void PandarGeneral_Internal::CalcQTPointXYZIT(HS_LIDAR_QT_Packet *pkt, int block char chLaserNumber, boost::shared_ptr cld) { HS_LIDAR_QT_Block *block = &pkt->blocks[blockid]; - struct tm tTm; + struct tm tTm = {0}; tTm.tm_year = pkt->addtime[0] + 100; // in case of time error @@ -1499,6 +1540,8 @@ void PandarGeneral_Internal::CalcQTPointXYZIT(HS_LIDAR_QT_Packet *pkt, int block tTm.tm_min = pkt->addtime[4]; tTm.tm_sec = pkt->addtime[5]; tTm.tm_isdst = 0; + tTm.tm_gmtoff = 0; + tTm.tm_zone = 0; double unix_second = \ static_cast(mktime(&tTm) + tz_second_); @@ -1513,16 +1556,15 @@ void PandarGeneral_Internal::CalcQTPointXYZIT(HS_LIDAR_QT_Packet *pkt, int block continue; } - double xyDistance = unit.distance * cosf(degreeToRadian(General_elev_angle_map_[i])); - - point.x = static_cast(xyDistance * \ - sinf(degreeToRadian(General_horizatal_azimuth_offset_map_[i] + \ - (static_cast(block->azimuth)) / 100.0))); - point.y = static_cast(xyDistance * \ - cosf(degreeToRadian(General_horizatal_azimuth_offset_map_[i] + \ - (static_cast(block->azimuth)) / 100.0))); - point.z = static_cast(unit.distance * \ - sinf(degreeToRadian(General_elev_angle_map_[i]))); + int azimuth = static_cast(General_horizatal_azimuth_offset_map_[i] * 100 + block->azimuth); + if(azimuth < 0) + azimuth += 36000; + if(azimuth > 36000) + azimuth -= 36000; + float xyDistance = unit.distance * cos_elevation_map[i]; + point.x = static_cast(xyDistance * sin_azimuth_map[azimuth]); + point.y = static_cast(xyDistance * cos_azimuth_map[azimuth]); + point.z = static_cast(unit.distance * sin_elevation_map[i]); point.intensity = unit.intensity; @@ -1551,7 +1593,7 @@ void PandarGeneral_Internal::CalcQTPointXYZIT(HS_LIDAR_QT_Packet *pkt, int block if (pcl_type_) PointCloudList[i].push_back(point); else - cld->push_back(point); + PointCloud.push_back(point); } } @@ -1559,7 +1601,7 @@ void PandarGeneral_Internal::CalcXTPointXYZIT(HS_LIDAR_XT_Packet *pkt, int block char chLaserNumber, boost::shared_ptr cld) { HS_LIDAR_XT_Block *block = &pkt->blocks[blockid]; - struct tm tTm; + struct tm tTm = {0}; tTm.tm_year = pkt->addtime[0] + 100; // in case of time error @@ -1574,6 +1616,8 @@ void PandarGeneral_Internal::CalcXTPointXYZIT(HS_LIDAR_XT_Packet *pkt, int block tTm.tm_min = pkt->addtime[4]; tTm.tm_sec = pkt->addtime[5]; tTm.tm_isdst = 0; + tTm.tm_gmtoff = 0; + tTm.tm_zone = 0; double unix_second = \ static_cast(mktime(&tTm) + tz_second_); @@ -1588,16 +1632,15 @@ void PandarGeneral_Internal::CalcXTPointXYZIT(HS_LIDAR_XT_Packet *pkt, int block continue; } - double xyDistance = unit.distance * cosf(degreeToRadian(General_elev_angle_map_[i])); - - point.x = static_cast(xyDistance * \ - sinf(degreeToRadian(General_horizatal_azimuth_offset_map_[i] + \ - (static_cast(block->azimuth)) / 100.0))); - point.y = static_cast(xyDistance * \ - cosf(degreeToRadian(General_horizatal_azimuth_offset_map_[i] + \ - (static_cast(block->azimuth)) / 100.0))); - point.z = static_cast(unit.distance * \ - sinf(degreeToRadian(General_elev_angle_map_[i]))); + int azimuth = static_cast(General_horizatal_azimuth_offset_map_[i] * 100 + block->azimuth); + if(azimuth < 0) + azimuth += 36000; + if(azimuth > 36000) + azimuth -= 36000; + float xyDistance = unit.distance * cos_elevation_map[i]; + point.x = static_cast(xyDistance * sin_azimuth_map[azimuth]); + point.y = static_cast(xyDistance * cos_azimuth_map[azimuth]); + point.z = static_cast(unit.distance * sin_elevation_map[i]); point.intensity = unit.intensity; @@ -1624,7 +1667,7 @@ void PandarGeneral_Internal::CalcXTPointXYZIT(HS_LIDAR_XT_Packet *pkt, int block if (pcl_type_) { PointCloudList[i].push_back(point); } else { - cld->push_back(point); + PointCloud.push_back(point); } } } @@ -1637,6 +1680,11 @@ void PandarGeneral_Internal::EmitBackMessege(char chLaserNumber, boost::shared_p } } } + else{ + cld->points.assign(PointCloud.begin(),PointCloud.end()); + cld->width = (uint32_t)cld->points.size(); + cld->height = 1; + } pcl_callback_(cld, cld->points[0].timestamp, scan); // the timestamp from first point cloud of cld //cld.reset(new PPointCloud()); if (pcl_type_) { @@ -1649,7 +1697,7 @@ void PandarGeneral_Internal::EmitBackMessege(char chLaserNumber, boost::shared_p void PandarGeneral_Internal::PushScanPacket(hesai_lidar::PandarScanPtr scan) { for(int i = 0; ipackets.size(); i++) { PandarPacket pkt; - pkt.stamp = scan->packets[i].stamp.sec + scan->packets[i].stamp.nsec * 1000000000.0; + pkt.stamp = scan->packets[i].stamp.sec + scan->packets[i].stamp.nsec / 1000000000.0; pkt.size = scan->packets[i].size; memcpy(&pkt.data[0], &(scan->packets[i].data[0]), scan->packets[i].size); PushLiDARData(pkt); diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h index d5fdc9b..7d900d0 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h @@ -390,6 +390,11 @@ class PandarGeneral_Internal { PcapReader *pcap_reader_; bool connect_lidar_; std::string m_sLidarType; + std::vector sin_azimuth_map; + std::vector cos_azimuth_map; + std::vector sin_elevation_map; + std::vector cos_elevation_map; + }; #endif // SRC_PANDARGENERAL_INTERNAL_H_ From 9c8a94fce0da284e112efe9bb11431c9b26e0ab9 Mon Sep 17 00:00:00 2001 From: zhangyu Date: Wed, 30 Dec 2020 07:48:16 +0000 Subject: [PATCH 2/9] Optimizing computational efficiency --- .../src/pandarGeneral_internal.cc | 133 ++++++++---------- .../src/pandarGeneral_internal.h | 8 +- .../src/PandarGeneralRaw/src/pcap_reader.cpp | 2 +- 3 files changed, 65 insertions(+), 78 deletions(-) diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc index a5cf974..ad2326f 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc @@ -79,7 +79,8 @@ static const float pandar20_horizatal_azimuth_offset_map[] = { }; static std::vector > PointCloudList(128); -static std::vector PointCloud; +static std::vector PointCloud(1000000); +static int iPointCloudIndex = 0; PandarGeneral_Internal::PandarGeneral_Internal( std::string device_ip, uint16_t lidar_port, uint16_t gps_port, @@ -140,11 +141,11 @@ PandarGeneral_Internal::PandarGeneral_Internal(std::string pcap_path, \ connect_lidar_ = false; m_sTimestampType = timestampType; m_dPktTimestamp = 0.0f; - sin_azimuth_map.resize(36000); - cos_azimuth_map.resize(36000); + m_sin_azimuth_map_.resize(36000); + m_cos_azimuth_map_.resize(36000); for(int i = 0; i < 36000; ++i) { - sin_azimuth_map[i] = sinf(i * M_PI / 18000); - cos_azimuth_map[i] = cosf(i * M_PI / 18000); + m_sin_azimuth_map_[i] = sinf(i * M_PI / 18000); + m_cos_azimuth_map_[i] = cosf(i * M_PI / 18000); } Init(); } @@ -442,11 +443,11 @@ void PandarGeneral_Internal::Init() { laserQTOffset_[63] = 10.0f + 136.45f; if (m_sLidarType == "Pandar40P" || m_sLidarType == "Pandar40M") { - sin_elevation_map.resize(LASER_COUNT); - cos_elevation_map.resize(LASER_COUNT); + m_sin_elevation_map_.resize(LASER_COUNT); + m_cos_elevation_map_.resize(LASER_COUNT); for (int i = 0; i < LASER_COUNT; i++) { - sin_elevation_map[i] = sinf(degreeToRadian(pandar40p_elev_angle_map[i])); - cos_elevation_map[i] = cosf(degreeToRadian(pandar40p_elev_angle_map[i])); + m_sin_elevation_map_[i] = sinf(degreeToRadian(pandar40p_elev_angle_map[i])); + m_cos_elevation_map_[i] = cosf(degreeToRadian(pandar40p_elev_angle_map[i])); General_elev_angle_map_[i] = pandar40p_elev_angle_map[i]; General_horizatal_azimuth_offset_map_[i] = \ pandar40p_horizatal_azimuth_offset_map[i]; @@ -454,11 +455,11 @@ void PandarGeneral_Internal::Init() { } if (m_sLidarType == "Pandar64") { - sin_elevation_map.resize(HS_LIDAR_L64_UNIT_NUM); - cos_elevation_map.resize(HS_LIDAR_L64_UNIT_NUM); + m_sin_elevation_map_.resize(HS_LIDAR_L64_UNIT_NUM); + m_cos_elevation_map_.resize(HS_LIDAR_L64_UNIT_NUM); for (int i = 0; i < HS_LIDAR_L64_UNIT_NUM; i++) { - sin_elevation_map[i] = sinf(degreeToRadian(pandarGeneral_elev_angle_map[i])); - cos_elevation_map[i] = cosf(degreeToRadian(pandarGeneral_elev_angle_map[i])); + m_sin_elevation_map_[i] = sinf(degreeToRadian(pandarGeneral_elev_angle_map[i])); + m_cos_elevation_map_[i] = cosf(degreeToRadian(pandarGeneral_elev_angle_map[i])); General_elev_angle_map_[i] = pandarGeneral_elev_angle_map[i]; General_horizatal_azimuth_offset_map_[i] = \ pandarGeneral_horizatal_azimuth_offset_map[i]; @@ -466,33 +467,33 @@ void PandarGeneral_Internal::Init() { } if (m_sLidarType == "Pandar20A" || m_sLidarType == "Pandar20B") { - sin_elevation_map.resize(HS_LIDAR_L20_UNIT_NUM); - cos_elevation_map.resize(HS_LIDAR_L20_UNIT_NUM); + m_sin_elevation_map_.resize(HS_LIDAR_L20_UNIT_NUM); + m_cos_elevation_map_.resize(HS_LIDAR_L20_UNIT_NUM); for (int i = 0; i < HS_LIDAR_L20_UNIT_NUM; i++) { - sin_elevation_map[i] = sinf(degreeToRadian(pandar20_elev_angle_map[i])); - cos_elevation_map[i] = cosf(degreeToRadian(pandar20_elev_angle_map[i])); + m_sin_elevation_map_[i] = sinf(degreeToRadian(pandar20_elev_angle_map[i])); + m_cos_elevation_map_[i] = cosf(degreeToRadian(pandar20_elev_angle_map[i])); General_elev_angle_map_[i] = pandar20_elev_angle_map[i]; General_horizatal_azimuth_offset_map_[i] = pandar20_horizatal_azimuth_offset_map[i]; } } if (m_sLidarType == "PandarQT") { - sin_elevation_map.resize(HS_LIDAR_QT_UNIT_NUM); - cos_elevation_map.resize(HS_LIDAR_QT_UNIT_NUM); + m_sin_elevation_map_.resize(HS_LIDAR_QT_UNIT_NUM); + m_cos_elevation_map_.resize(HS_LIDAR_QT_UNIT_NUM); for (int i = 0; i < HS_LIDAR_QT_UNIT_NUM; i++) { - sin_elevation_map[i] = sinf(degreeToRadian(pandarQT_elev_angle_map[i])); - cos_elevation_map[i] = cosf(degreeToRadian(pandarQT_elev_angle_map[i])); + m_sin_elevation_map_[i] = sinf(degreeToRadian(pandarQT_elev_angle_map[i])); + m_cos_elevation_map_[i] = cosf(degreeToRadian(pandarQT_elev_angle_map[i])); General_elev_angle_map_[i] = pandarQT_elev_angle_map[i]; General_horizatal_azimuth_offset_map_[i] = pandarQT_horizatal_azimuth_offset_map[i]; } } if (m_sLidarType == "PandarXT-32") { - sin_elevation_map.resize(HS_LIDAR_XT_UNIT_NUM); - cos_elevation_map.resize(HS_LIDAR_XT_UNIT_NUM); + m_sin_elevation_map_.resize(HS_LIDAR_XT_UNIT_NUM); + m_cos_elevation_map_.resize(HS_LIDAR_XT_UNIT_NUM); for (int i = 0; i < HS_LIDAR_XT_UNIT_NUM; i++) { - sin_elevation_map[i] = sinf(degreeToRadian(pandarXT_elev_angle_map[i])); - cos_elevation_map[i] = cosf(degreeToRadian(pandarXT_elev_angle_map[i])); + m_sin_elevation_map_[i] = sinf(degreeToRadian(pandarXT_elev_angle_map[i])); + m_cos_elevation_map_[i] = cosf(degreeToRadian(pandarXT_elev_angle_map[i])); General_elev_angle_map_[i] = pandarXT_elev_angle_map[i]; General_horizatal_azimuth_offset_map_[i] = pandarXT_horizatal_azimuth_offset_map[i]; laserXTOffset_[i] = laserXTOffset[i]; @@ -500,11 +501,11 @@ void PandarGeneral_Internal::Init() { } if (m_sLidarType == "PandarXT-16") { - sin_elevation_map.resize(HS_LIDAR_XT16_UNIT_NUM); - cos_elevation_map.resize(HS_LIDAR_XT16_UNIT_NUM); + m_sin_elevation_map_.resize(HS_LIDAR_XT16_UNIT_NUM); + m_cos_elevation_map_.resize(HS_LIDAR_XT16_UNIT_NUM); for (int i = 0; i < HS_LIDAR_XT16_UNIT_NUM; i++) { - sin_elevation_map[i] = sinf(degreeToRadian(pandarXT_elev_angle_map[i*2])); - cos_elevation_map[i] = cosf(degreeToRadian(pandarXT_elev_angle_map[i*2])); + m_sin_elevation_map_[i] = sinf(degreeToRadian(pandarXT_elev_angle_map[i*2])); + m_cos_elevation_map_[i] = cosf(degreeToRadian(pandarXT_elev_angle_map[i*2])); General_elev_angle_map_[i] = pandarXT_elev_angle_map[i*2]; General_horizatal_azimuth_offset_map_[i] = pandarXT_horizatal_azimuth_offset_map[i*2]; laserXTOffset_[i] = laserXTOffset[i*2]; @@ -561,13 +562,13 @@ int PandarGeneral_Internal::LoadCorrectionFile(std::string correction_content) { elev_angle[lineId - 1] = elev; azimuthOffset[lineId - 1] = azimuth; } - sin_elevation_map.resize(lineCounter); - cos_elevation_map.resize(lineCounter); + m_sin_elevation_map_.resize(lineCounter); + m_cos_elevation_map_.resize(lineCounter); for (int i = 0; i < lineCounter; ++i) { /* for all the laser offset */ General_elev_angle_map_[i] = elev_angle[i]; - sin_elevation_map[i] = sinf(degreeToRadian(General_elev_angle_map_[i])); - cos_elevation_map[i] = cosf(degreeToRadian(General_elev_angle_map_[i])); + m_sin_elevation_map_[i] = sinf(degreeToRadian(General_elev_angle_map_[i])); + m_cos_elevation_map_[i] = cosf(degreeToRadian(General_elev_angle_map_[i])); General_horizatal_azimuth_offset_map_[i] = azimuthOffset[i]; } @@ -662,7 +663,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { boost::shared_ptr outMsg(new PPointCloud()); outMsg->points.resize(1); - PointCloud.reserve(70000); hesai_lidar::PandarScanPtr scan(new hesai_lidar::PandarScan); hesai_lidar::PandarPacket rawpacket; @@ -753,8 +753,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); - PointCloud.clear(); - PointCloud.reserve(70000); } } } else { @@ -791,8 +789,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); - PointCloud.clear(); - PointCloud.reserve(70000); } } } else { @@ -828,8 +824,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); - PointCloud.clear(); - PointCloud.reserve(70000); } } } else { @@ -866,8 +860,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); - PointCloud.clear(); - PointCloud.reserve(70000); } } } else { @@ -893,7 +885,7 @@ void PandarGeneral_Internal::PushLiDARData(PandarPacket packet) { } void PandarGeneral_Internal::ProcessGps(const PandarGPS &gpsMsg) { - struct tm t; + struct tm t = {0}; t.tm_sec = gpsMsg.second; t.tm_min = gpsMsg.minute; @@ -1315,10 +1307,10 @@ void PandarGeneral_Internal::CalcPointXYZIT(Pandar40PPacket *pkt, int blockid, azimuth += 36000; if(azimuth > 36000) azimuth -= 36000; - float xyDistance = unit.distance * cos_elevation_map[i]; - point.x = static_cast(xyDistance * sin_azimuth_map[azimuth]); - point.y = static_cast(xyDistance * cos_azimuth_map[azimuth]); - point.z = static_cast(unit.distance * sin_elevation_map[i]); + float xyDistance = unit.distance * m_cos_elevation_map_[i]; + point.x = static_cast(xyDistance * m_sin_azimuth_map_[azimuth]); + point.y = static_cast(xyDistance * m_cos_azimuth_map_[azimuth]); + point.z = static_cast(unit.distance * m_sin_elevation_map_[i]); point.intensity = unit.intensity; @@ -1372,8 +1364,6 @@ void PandarGeneral_Internal::CalcL64PointXYZIT(HS_LIDAR_L64_Packet *pkt, int blo tTm.tm_min = pkt->addtime[4]; tTm.tm_sec = pkt->addtime[5]; tTm.tm_isdst = 0; - tTm.tm_gmtoff = 0; - tTm.tm_zone = 0; double unix_second = \ static_cast(mktime(&tTm) + tz_second_); @@ -1393,10 +1383,10 @@ void PandarGeneral_Internal::CalcL64PointXYZIT(HS_LIDAR_L64_Packet *pkt, int blo azimuth += 36000; if(azimuth > 36000) azimuth -= 36000; - float xyDistance = unit.distance * cos_elevation_map[i]; - point.x = static_cast(xyDistance * sin_azimuth_map[azimuth]); - point.y = static_cast(xyDistance * cos_azimuth_map[azimuth]); - point.z = static_cast(unit.distance * sin_elevation_map[i]); + float xyDistance = unit.distance * m_cos_elevation_map_[i]; + point.x = static_cast(xyDistance * m_sin_azimuth_map_[azimuth]); + point.y = static_cast(xyDistance * m_cos_azimuth_map_[azimuth]); + point.z = static_cast(unit.distance * m_sin_elevation_map_[i]); point.intensity = unit.intensity; @@ -1451,8 +1441,6 @@ void PandarGeneral_Internal::CalcL20PointXYZIT(HS_LIDAR_L20_Packet *pkt, int blo tTm.tm_min = pkt->addtime[4]; tTm.tm_sec = pkt->addtime[5]; tTm.tm_isdst = 0; - tTm.tm_gmtoff = 0; - tTm.tm_zone = 0; double unix_second = \ static_cast(mktime(&tTm) + tz_second_); @@ -1472,10 +1460,10 @@ void PandarGeneral_Internal::CalcL20PointXYZIT(HS_LIDAR_L20_Packet *pkt, int blo azimuth += 36000; if(azimuth > 36000) azimuth -= 36000; - float xyDistance = unit.distance * cos_elevation_map[i]; - point.x = static_cast(xyDistance * sin_azimuth_map[azimuth]); - point.y = static_cast(xyDistance * cos_azimuth_map[azimuth]); - point.z = static_cast(unit.distance * sin_elevation_map[i]); + float xyDistance = unit.distance * m_cos_elevation_map_[i]; + point.x = static_cast(xyDistance * m_sin_azimuth_map_[azimuth]); + point.y = static_cast(xyDistance * m_cos_azimuth_map_[azimuth]); + point.z = static_cast(unit.distance * m_sin_elevation_map_[i]); point.intensity = unit.intensity; @@ -1540,8 +1528,6 @@ void PandarGeneral_Internal::CalcQTPointXYZIT(HS_LIDAR_QT_Packet *pkt, int block tTm.tm_min = pkt->addtime[4]; tTm.tm_sec = pkt->addtime[5]; tTm.tm_isdst = 0; - tTm.tm_gmtoff = 0; - tTm.tm_zone = 0; double unix_second = \ static_cast(mktime(&tTm) + tz_second_); @@ -1561,10 +1547,10 @@ void PandarGeneral_Internal::CalcQTPointXYZIT(HS_LIDAR_QT_Packet *pkt, int block azimuth += 36000; if(azimuth > 36000) azimuth -= 36000; - float xyDistance = unit.distance * cos_elevation_map[i]; - point.x = static_cast(xyDistance * sin_azimuth_map[azimuth]); - point.y = static_cast(xyDistance * cos_azimuth_map[azimuth]); - point.z = static_cast(unit.distance * sin_elevation_map[i]); + float xyDistance = unit.distance * m_cos_elevation_map_[i]; + point.x = static_cast(xyDistance * m_sin_azimuth_map_[azimuth]); + point.y = static_cast(xyDistance * m_cos_azimuth_map_[azimuth]); + point.z = static_cast(unit.distance * m_sin_elevation_map_[i]); point.intensity = unit.intensity; @@ -1616,8 +1602,6 @@ void PandarGeneral_Internal::CalcXTPointXYZIT(HS_LIDAR_XT_Packet *pkt, int block tTm.tm_min = pkt->addtime[4]; tTm.tm_sec = pkt->addtime[5]; tTm.tm_isdst = 0; - tTm.tm_gmtoff = 0; - tTm.tm_zone = 0; double unix_second = \ static_cast(mktime(&tTm) + tz_second_); @@ -1637,10 +1621,10 @@ void PandarGeneral_Internal::CalcXTPointXYZIT(HS_LIDAR_XT_Packet *pkt, int block azimuth += 36000; if(azimuth > 36000) azimuth -= 36000; - float xyDistance = unit.distance * cos_elevation_map[i]; - point.x = static_cast(xyDistance * sin_azimuth_map[azimuth]); - point.y = static_cast(xyDistance * cos_azimuth_map[azimuth]); - point.z = static_cast(unit.distance * sin_elevation_map[i]); + float xyDistance = unit.distance * m_cos_elevation_map_[i]; + point.x = static_cast(xyDistance * m_sin_azimuth_map_[azimuth]); + point.y = static_cast(xyDistance * m_cos_azimuth_map_[azimuth]); + point.z = static_cast(unit.distance * m_sin_elevation_map_[i]); point.intensity = unit.intensity; @@ -1667,7 +1651,9 @@ void PandarGeneral_Internal::CalcXTPointXYZIT(HS_LIDAR_XT_Packet *pkt, int block if (pcl_type_) { PointCloudList[i].push_back(point); } else { - PointCloud.push_back(point); + PointCloud[iPointCloudIndex] = point; + iPointCloudIndex++; + // PointCloud.emplace_back(point); } } } @@ -1681,9 +1667,10 @@ void PandarGeneral_Internal::EmitBackMessege(char chLaserNumber, boost::shared_p } } else{ - cld->points.assign(PointCloud.begin(),PointCloud.end()); + cld->points.assign(PointCloud.begin(),PointCloud.begin() + iPointCloudIndex + 1); cld->width = (uint32_t)cld->points.size(); cld->height = 1; + iPointCloudIndex = 0; } pcl_callback_(cld, cld->points[0].timestamp, scan); // the timestamp from first point cloud of cld //cld.reset(new PPointCloud()); diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h index 7d900d0..9afff6c 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h @@ -390,10 +390,10 @@ class PandarGeneral_Internal { PcapReader *pcap_reader_; bool connect_lidar_; std::string m_sLidarType; - std::vector sin_azimuth_map; - std::vector cos_azimuth_map; - std::vector sin_elevation_map; - std::vector cos_elevation_map; + std::vector m_sin_azimuth_map_; + std::vector m_cos_azimuth_map_; + std::vector m_sin_elevation_map_; + std::vector m_cos_elevation_map_; }; diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pcap_reader.cpp b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pcap_reader.cpp index 15b1d06..e1f3226 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pcap_reader.cpp +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pcap_reader.cpp @@ -69,7 +69,7 @@ void PcapReader::parsePcap() { struct bpf_program filter; pcap_pkthdr *pktHeader; const unsigned char *packetBuf; - struct tm t; + struct tm t = {0}; static int gap = 100; int64_t last_pkt_ts = 0; int count; From c9166a37d1f55d93525eb005c02fb3593eb7570d Mon Sep 17 00:00:00 2001 From: zhangyu Date: Thu, 31 Dec 2020 14:49:55 +0800 Subject: [PATCH 3/9] Optimizing computational efficiency --- README.md | 3 ++ .../src/pandarGeneral_internal.cc | 42 ++++++++++--------- 2 files changed, 25 insertions(+), 20 deletions(-) diff --git a/README.md b/README.md index 429eadf..d6060ee 100644 --- a/README.md +++ b/README.md @@ -73,6 +73,9 @@ Data source will be from pcap file once "pcap_file" not empty $ source install/setup.bash ``` ``` +$ export TZ=UTC0 +``` +``` for PandarQT $ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="PandarQT" for Pandar64 diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc index ad2326f..15f3e94 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc @@ -141,12 +141,6 @@ PandarGeneral_Internal::PandarGeneral_Internal(std::string pcap_path, \ connect_lidar_ = false; m_sTimestampType = timestampType; m_dPktTimestamp = 0.0f; - m_sin_azimuth_map_.resize(36000); - m_cos_azimuth_map_.resize(36000); - for(int i = 0; i < 36000; ++i) { - m_sin_azimuth_map_[i] = sinf(i * M_PI / 18000); - m_cos_azimuth_map_[i] = cosf(i * M_PI / 18000); - } Init(); } @@ -167,6 +161,12 @@ void PandarGeneral_Internal::Init() { cos_lookup_table_[rotIndex] = cosf(rotation); sin_lookup_table_[rotIndex] = sinf(rotation); } + m_sin_azimuth_map_.resize(36000); + m_cos_azimuth_map_.resize(36000); + for(int i = 0; i < 36000; ++i) { + m_sin_azimuth_map_[i] = sinf(i * M_PI / 18000); + m_cos_azimuth_map_[i] = cosf(i * M_PI / 18000); + } //laser40 // init the block time offset, us @@ -662,7 +662,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { int ret = 0; boost::shared_ptr outMsg(new PPointCloud()); - outMsg->points.resize(1); hesai_lidar::PandarScanPtr scan(new hesai_lidar::PandarScan); hesai_lidar::PandarPacket rawpacket; @@ -713,10 +712,10 @@ void PandarGeneral_Internal::ProcessLiarPacket() { start_angle_ <= pkt.blocks[i].azimuth) || (last_azimuth_ < start_angle_ && start_angle_ <= pkt.blocks[i].azimuth)) { - if (pcl_callback_ && (outMsg->points.size() > 0 || PointCloudList[0].size() > 0)) { + if (pcl_callback_ && (iPointCloudIndex > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(LASER_COUNT, outMsg, scan); scan->packets.clear(); - outMsg.reset(new PPointCloud()); + scan->packets.reserve(600); } } } @@ -749,7 +748,7 @@ void PandarGeneral_Internal::ProcessLiarPacket() { start_angle_ <= pkt.blocks[i].azimuth) || (last_azimuth_ < start_angle_ && start_angle_ <= pkt.blocks[i].azimuth)) { - if (pcl_callback_ && (outMsg->points.size() > 0 || PointCloudList[0].size() > 0)) { + if (pcl_callback_ && (iPointCloudIndex > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); @@ -785,7 +784,7 @@ void PandarGeneral_Internal::ProcessLiarPacket() { start_angle_ <= pkt.blocks[i].azimuth) || (last_azimuth_ < start_angle_ && start_angle_ <= pkt.blocks[i].azimuth)) { - if (pcl_callback_ && (outMsg->points.size() > 0 || PointCloudList[0].size() > 0)) { + if (pcl_callback_ && (iPointCloudIndex > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); @@ -820,7 +819,7 @@ void PandarGeneral_Internal::ProcessLiarPacket() { start_angle_ <= pkt.blocks[i].azimuth) || (last_azimuth_ < start_angle_ && start_angle_ <= pkt.blocks[i].azimuth)) { - if (pcl_callback_ && (outMsg->points.size() > 0 || PointCloudList[0].size() > 0)) { + if (pcl_callback_ && (iPointCloudIndex > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); @@ -856,7 +855,7 @@ void PandarGeneral_Internal::ProcessLiarPacket() { start_angle_ <= pkt.blocks[i].azimuth) || (last_azimuth_ < start_angle_ && start_angle_ <= pkt.blocks[i].azimuth)) { - if (pcl_callback_ && (outMsg->points.size() > 0 || PointCloudList[0].size() > 0)) { + if (pcl_callback_ && (iPointCloudIndex > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); @@ -1338,7 +1337,8 @@ void PandarGeneral_Internal::CalcPointXYZIT(Pandar40PPacket *pkt, int blockid, if (pcl_type_) { PointCloudList[i].push_back(point); } else { - PointCloud.push_back(point); + PointCloud[iPointCloudIndex] = point; + iPointCloudIndex++; } } } @@ -1415,7 +1415,8 @@ void PandarGeneral_Internal::CalcL64PointXYZIT(HS_LIDAR_L64_Packet *pkt, int blo if (pcl_type_) { PointCloudList[i].push_back(point); } else { - PointCloud.push_back(point); + PointCloud[iPointCloudIndex] = point; + iPointCloudIndex++; } } } @@ -1503,7 +1504,8 @@ void PandarGeneral_Internal::CalcL20PointXYZIT(HS_LIDAR_L20_Packet *pkt, int blo if (pcl_type_) { PointCloudList[i].push_back(point); } else { - PointCloud.push_back(point); + PointCloud[iPointCloudIndex] = point; + iPointCloudIndex++; } } } @@ -1579,7 +1581,8 @@ void PandarGeneral_Internal::CalcQTPointXYZIT(HS_LIDAR_QT_Packet *pkt, int block if (pcl_type_) PointCloudList[i].push_back(point); else - PointCloud.push_back(point); + PointCloud[iPointCloudIndex] = point; + iPointCloudIndex++; } } @@ -1653,7 +1656,6 @@ void PandarGeneral_Internal::CalcXTPointXYZIT(HS_LIDAR_XT_Packet *pkt, int block } else { PointCloud[iPointCloudIndex] = point; iPointCloudIndex++; - // PointCloud.emplace_back(point); } } } @@ -1667,13 +1669,13 @@ void PandarGeneral_Internal::EmitBackMessege(char chLaserNumber, boost::shared_p } } else{ - cld->points.assign(PointCloud.begin(),PointCloud.begin() + iPointCloudIndex + 1); + cld->points.assign(PointCloud.begin(),PointCloud.begin() + iPointCloudIndex); cld->width = (uint32_t)cld->points.size(); cld->height = 1; iPointCloudIndex = 0; } pcl_callback_(cld, cld->points[0].timestamp, scan); // the timestamp from first point cloud of cld - //cld.reset(new PPointCloud()); + cld.reset(new PPointCloud()); if (pcl_type_) { for (int i=0; i<128; i++) { PointCloudList[i].clear(); From 49785d12be9dd5145833dadc728b080e65b65055 Mon Sep 17 00:00:00 2001 From: zhangyu Date: Thu, 31 Dec 2020 16:00:57 +0800 Subject: [PATCH 4/9] Optimizing computational efficiency in pcl type --- .../PandarGeneralRaw/src/pandarGeneral_internal.cc | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc index 15f3e94..9542b55 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc @@ -167,6 +167,11 @@ void PandarGeneral_Internal::Init() { m_sin_azimuth_map_[i] = sinf(i * M_PI / 18000); m_cos_azimuth_map_[i] = cosf(i * M_PI / 18000); } + if (pcl_type_) { + for (int i = 0; i < 128; i++) { + PointCloudList[i].reserve(10000); + } + } //laser40 // init the block time offset, us @@ -716,6 +721,7 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(LASER_COUNT, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); + outMsg.reset(new PPointCloud()); } } } @@ -752,6 +758,7 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); + outMsg.reset(new PPointCloud()); } } } else { @@ -788,6 +795,7 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); + outMsg.reset(new PPointCloud()); } } } else { @@ -823,6 +831,7 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); + outMsg.reset(new PPointCloud()); } } } else { @@ -859,6 +868,7 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); + outMsg.reset(new PPointCloud()); } } } else { @@ -1675,10 +1685,10 @@ void PandarGeneral_Internal::EmitBackMessege(char chLaserNumber, boost::shared_p iPointCloudIndex = 0; } pcl_callback_(cld, cld->points[0].timestamp, scan); // the timestamp from first point cloud of cld - cld.reset(new PPointCloud()); if (pcl_type_) { for (int i=0; i<128; i++) { PointCloudList[i].clear(); + PointCloudList[i].reserve(10000); } } } From 665bcf06e4625c9a2bd39d951e4043ece2afcf44 Mon Sep 17 00:00:00 2001 From: zhangyu-hesaitech Date: Tue, 5 Jan 2021 16:11:20 +0800 Subject: [PATCH 5/9] Optimizing computational efficiency --- .../src/PandarGeneralRaw/src/pandarGeneral_internal.cc | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc index 9542b55..6c58d67 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc @@ -79,6 +79,7 @@ static const float pandar20_horizatal_azimuth_offset_map[] = { }; static std::vector > PointCloudList(128); +// static std::vector PointCloud; static std::vector PointCloud(1000000); static int iPointCloudIndex = 0; @@ -721,7 +722,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(LASER_COUNT, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); - outMsg.reset(new PPointCloud()); } } } @@ -758,7 +758,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); - outMsg.reset(new PPointCloud()); } } } else { @@ -795,7 +794,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); - outMsg.reset(new PPointCloud()); } } } else { @@ -831,7 +829,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); - outMsg.reset(new PPointCloud()); } } } else { @@ -868,7 +865,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); scan->packets.reserve(600); - outMsg.reset(new PPointCloud()); } } } else { @@ -1689,6 +1685,9 @@ void PandarGeneral_Internal::EmitBackMessege(char chLaserNumber, boost::shared_p for (int i=0; i<128; i++) { PointCloudList[i].clear(); PointCloudList[i].reserve(10000); + cld->points.clear(); + cld->width = (uint32_t)cld->points.size(); + cld->height = 1; } } } From 37ef2d6d193879d178ca9b9720908f78c05b1080 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BC=A0=E7=85=9C?= Date: Tue, 5 Jan 2021 16:52:00 +0800 Subject: [PATCH 6/9] change vwesion --- change notes.md | 10 +++++++++- .../src/PandarGeneralRaw/include/version.h | 2 +- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/change notes.md b/change notes.md index 266fdcf..af6a51f 100644 --- a/change notes.md +++ b/change notes.md @@ -20,4 +20,12 @@ PandarGeneralROS_1.1.1 PandarGeneralROS_1.1.2 ##modify -1. modify msg data size \ No newline at end of file +1. modify msg data size + +星期二, 05. 一月 2021 17:00下午 +##version +PandarGeneralROS_1.1.3 + +##modify +1. fix the problem in playing rosbag when timestamp is realtime +2. optimize computational efficiency \ No newline at end of file diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/include/version.h b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/include/version.h index 421edaa..cd0990d 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/include/version.h +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/include/version.h @@ -21,7 +21,7 @@ #include #include -#define VERSION "PandarGeneralSDK_1.1.2" +#define VERSION "PandarGeneralSDK_1.1.3" #ifdef __cplusplus extern "C" { #endif From 2ee41cdf47e6d1d6ac6d8d557172d862987f0bed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BC=A0=E7=85=9C?= Date: Tue, 5 Jan 2021 21:13:55 +0800 Subject: [PATCH 7/9] add SetEnvironmentVariableTZ function --- README.md | 3 -- .../src/pandarGeneral_internal.cc | 47 +++++++++++++++---- .../src/pandarGeneral_internal.h | 4 ++ 3 files changed, 42 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index d6060ee..429eadf 100644 --- a/README.md +++ b/README.md @@ -73,9 +73,6 @@ Data source will be from pcap file once "pcap_file" not empty $ source install/setup.bash ``` ``` -$ export TZ=UTC0 -``` -``` for PandarQT $ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="PandarQT" for Pandar64 diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc index 6c58d67..cc55d27 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc @@ -78,9 +78,9 @@ static const float pandar20_horizatal_azimuth_offset_map[] = { -1.042f, -1.042f, -1.042f, -1.042f }; -static std::vector > PointCloudList(128); +static std::vector > PointCloudList(MAX_LASER_NUM); // static std::vector PointCloud; -static std::vector PointCloud(1000000); +static std::vector PointCloud(MAX_POINT_CLOUD_NUM); static int iPointCloudIndex = 0; PandarGeneral_Internal::PandarGeneral_Internal( @@ -162,15 +162,15 @@ void PandarGeneral_Internal::Init() { cos_lookup_table_[rotIndex] = cosf(rotation); sin_lookup_table_[rotIndex] = sinf(rotation); } - m_sin_azimuth_map_.resize(36000); - m_cos_azimuth_map_.resize(36000); - for(int i = 0; i < 36000; ++i) { + m_sin_azimuth_map_.resize(MAX_AZIMUTH_DEGREE_NUM); + m_cos_azimuth_map_.resize(MAX_AZIMUTH_DEGREE_NUM); + for(int i = 0; i < MAX_AZIMUTH_DEGREE_NUM; ++i) { m_sin_azimuth_map_[i] = sinf(i * M_PI / 18000); m_cos_azimuth_map_[i] = cosf(i * M_PI / 18000); } if (pcl_type_) { - for (int i = 0; i < 128; i++) { - PointCloudList[i].reserve(10000); + for (int i = 0; i < MAX_LASER_NUM; i++) { + PointCloudList[i].reserve(MAX_POINT_CLOUD_NUM_PER_CHANNEL); } } @@ -522,6 +522,7 @@ void PandarGeneral_Internal::Init() { blockXTOffsetSingle_[i] = blockXTOffsetSingle[i]; blockXTOffsetDual_[i] = blockXTOffsetDual[i]; } + SetEnvironmentVariableTZ(); } /** @@ -1682,9 +1683,9 @@ void PandarGeneral_Internal::EmitBackMessege(char chLaserNumber, boost::shared_p } pcl_callback_(cld, cld->points[0].timestamp, scan); // the timestamp from first point cloud of cld if (pcl_type_) { - for (int i=0; i<128; i++) { + for (int i=0; ipoints.clear(); cld->width = (uint32_t)cld->points.size(); cld->height = 1; @@ -1702,4 +1703,32 @@ void PandarGeneral_Internal::PushScanPacket(hesai_lidar::PandarScanPtr scan) { } } +void PandarGeneral_Internal::SetEnvironmentVariableTZ(){ + char *TZ; + if((TZ = getenv("TZ"))){ + printf("TZ=%s\n",TZ); + return; + } + unsigned int timezone = 0; + time_t t1, t2 ; + struct tm *tm_local, *tm_utc; + time(&t1); + t2 = t1; + tm_local = localtime(&t1); + t1 = mktime(tm_local) ; + tm_utc = gmtime(&t2); + t2 = mktime(tm_utc); + timezone = (t1 - t2) / 3600; + std::string data = "TZ=UTC" + std::to_string(timezone); + int len = data.length(); + TZ = (char *)malloc((len + 1) * sizeof(char)); + data.copy(TZ, len, 0); + if(putenv(TZ) == 0){ + printf("set environment %s\n", TZ); + } + else{ + printf("set environment fail\n"); + } +} + diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h index 9afff6c..e210d1a 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h @@ -136,6 +136,9 @@ HS_LIDAR_L64_7_BLOCK_PACKET_BODY_SIZE + HS_LIDAR_L64_PACKET_TAIL_WITHOUT_UDPSEQ_ #define HesaiLidarSDK_DEFAULT_GPS_RECV_PORT 10110 #define MAX_LASER_NUM (256) +#define MAX_POINT_CLOUD_NUM (1000000) +#define MAX_POINT_CLOUD_NUM_PER_CHANNEL (10000) +#define MAX_AZIMUTH_DEGREE_NUM (36000) struct Pandar40PUnit_s { uint8_t intensity; @@ -326,6 +329,7 @@ class PandarGeneral_Internal { void FillPacket(const uint8_t *buf, const int len, double timestamp); void EmitBackMessege(char chLaserNumber, boost::shared_ptr cld, hesai_lidar::PandarScanPtr scan); + void SetEnvironmentVariableTZ(); pthread_mutex_t lidar_lock_; sem_t lidar_sem_; boost::thread *lidar_recv_thr_; From 4d2eebe541cf48d9057bcb988348daedcee9dd92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BC=A0=E7=85=9C?= Date: Wed, 6 Jan 2021 11:05:38 +0800 Subject: [PATCH 8/9] fix SetEnvironmentVariableTZ --- .../src/pandarGeneral_internal.cc | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc index cc55d27..edf9eaf 100644 --- a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc +++ b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc @@ -722,7 +722,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { if (pcl_callback_ && (iPointCloudIndex > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(LASER_COUNT, outMsg, scan); scan->packets.clear(); - scan->packets.reserve(600); } } } @@ -758,7 +757,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { if (pcl_callback_ && (iPointCloudIndex > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); - scan->packets.reserve(600); } } } else { @@ -794,7 +792,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { if (pcl_callback_ && (iPointCloudIndex > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); - scan->packets.reserve(600); } } } else { @@ -829,7 +826,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { if (pcl_callback_ && (iPointCloudIndex > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); - scan->packets.reserve(600); } } } else { @@ -865,7 +861,6 @@ void PandarGeneral_Internal::ProcessLiarPacket() { if (pcl_callback_ && (iPointCloudIndex > 0 || PointCloudList[0].size() > 0)) { EmitBackMessege(pkt.header.chLaserNumber, outMsg, scan); scan->packets.clear(); - scan->packets.reserve(600); } } } else { @@ -1712,13 +1707,13 @@ void PandarGeneral_Internal::SetEnvironmentVariableTZ(){ unsigned int timezone = 0; time_t t1, t2 ; struct tm *tm_local, *tm_utc; - time(&t1); - t2 = t1; - tm_local = localtime(&t1); - t1 = mktime(tm_local) ; - tm_utc = gmtime(&t2); - t2 = mktime(tm_utc); - timezone = (t1 - t2) / 3600; + time(&t1); + t2 = t1; + tm_local = localtime(&t1); + t1 = mktime(tm_local) ; + tm_utc = gmtime(&t2); + t2 = mktime(tm_utc); + timezone = t2 >= t1 ? (t2 - t1) / 3600 : (t1 - t2) / 3600; std::string data = "TZ=UTC" + std::to_string(timezone); int len = data.length(); TZ = (char *)malloc((len + 1) * sizeof(char)); From 5a418e3f7a0e49be09f7850fa87f442b18f6f251 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BC=A0=E7=85=9C?= Date: Wed, 6 Jan 2021 13:05:23 +0800 Subject: [PATCH 9/9] change readme --- README.md | 28 +++++++++++++++++++--------- change notes.md | 3 ++- launch/hesai_lidar.launch | 2 +- 3 files changed, 22 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 429eadf..3d73402 100644 --- a/README.md +++ b/README.md @@ -65,6 +65,16 @@ Make sure parameters above set to the same with Lidar setting Data source will be from pcap file once "pcap_file" not empty +**Reciving data from rosbag: config data_type and publish_type,leave the pcap_file empty** +|Parameter | Value| +|---------|---------------| +|pcap_file || +|publish_type |points|　 +|data_type |rosbag| + +Data source will be from rosbag when "pcap_file" is set to empty and "data_type" is set to "rosbag" +Make sure the parameter "publish_type" is set to "points" +Make sure the parameter "namespace" in file hesai_lidar.launch is same with the namespace in rosbag ## Run @@ -74,22 +84,22 @@ $ source install/setup.bash ``` ``` for PandarQT -$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="PandarQT" +$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="PandarQT" frame_id:="PandarQT" for Pandar64 -$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="Pandar64" +$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="Pandar64" frame_id:="Pandar64" for Pandar20A -$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="Pandar20A" +$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="Pandar20A" frame_id:="Pandar20A" for Pandar20B -$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="Pandar20B" +$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="Pandar20B" frame_id:="Pandar20B" for Pandar40P -$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="Pandar40P" +$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="Pandar40P" frame_id:="Pandar40P" for Pandar40M -$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="Pandar40M" +$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="Pandar40M" frame_id:="Pandar40M" for PandarXT-32 -$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="PandarXT-32" +$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="PandarXT-32" frame_id:="PandarXT-32" for PandarXT-16 -$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="PandarXT-16" +$ roslaunch hesai_lidar hesai_lidar.launch lidar_type:="PandarXT-16" frame_id:="PandarXT-16" ``` 2. The driver will publish PointCloud messages to the topic `/pandar` 3. Open Rviz and add display by topic -4. Change fixed frame to lidar_type to view published point clouds +4. Change fixed frame to frame_id to view published point clouds diff --git a/change notes.md b/change notes.md index af6a51f..92d1e09 100644 --- a/change notes.md +++ b/change notes.md @@ -28,4 +28,5 @@ PandarGeneralROS_1.1.3 ##modify 1. fix the problem in playing rosbag when timestamp is realtime -2. optimize computational efficiency \ No newline at end of file +2. optimize computational efficiency +3. change readme \ No newline at end of file diff --git a/launch/hesai_lidar.launch b/launch/hesai_lidar.launch index 8942948..846a97d 100644 --- a/launch/hesai_lidar.launch +++ b/launch/hesai_lidar.launch @@ -12,7 +12,7 @@ - +