Merge branch 'dev' into 'master'

V1.0.37 fix bug in calculate lidar work mode

See merge request SoftwareTeam/GeneralSoftware/HesaiLidar_Swift_ROS!71

Author: zhangyu-hesaitech

README.md

@@ -9,7 +9,7 @@ Developed based on [HesaiLidar_Swift_SDK](https://github.com/HesaiTechnology/Hes
 ```
 * master: Pandar LiDAR ROS driver for Ubuntu 18.04 and Ubuntu20.04 supports the latest UDP protocol v1.4 v1.3 and v3.2 
 * UDP1.4_ubuntu16.04: Pandar LiDAR ROS driver for Ubuntu 16.04 supports the latest UDP protocol v1.4
-* UDP1.3: Pandar LiDAR ROS driver for ubuntu16.04,ubuntu 18.04 and Ubuntu 20.04 supports UDP protocol v1.3 
+* UDP4.3: Pandar LiDAR ROS driver for  Ubuntu 16.04/Ubuntu 18.04/Ubuntu 20.04 supports the latest UDP protocol v4.3 
 
 To get the UDP protocol version number of your device,  check the UDP package header field.
 ```

change notes.md

@@ -146,5 +146,12 @@ PandarSwiftROS_1.0.36
 ##modify
 1. Support multicast
 
+Wednesday December 14th, 2022 17:30
+##version
+PandarSwiftROS_1.0.37
+
+##modify
+1. Support save pcd
+
 
 

pandar_pointcloud/src/conversions/convert.cc

@@ -86,7 +86,7 @@ Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh,
     : data_(new pandar_rawdata::RawData()),
       drv(node, private_nh, node_type, this) {
 
-  m_sRosVersion = "PandarSwiftROS_1.0.36";
+  m_sRosVersion = "PandarSwiftROS_1.0.37";
   ROS_WARN("--------PandarSwift ROS version: %s--------\n\n",m_sRosVersion.c_str());
 
   publishmodel = "";
@@ -344,7 +344,7 @@ int Convert::loadChannelConfigFile(std::string channel_config_content){
       m_PandarQTChannelConfig.m_u8MinVersion = std::stoi(versionLine[2].c_str());
   }
   else{
-      std::cout << "channel config file delimiter is wrong" << versionLine[0];
+      std::cout << "channel config file delimiter is wrong" << versionLine[0] << std::endl;
       return -1;
   }
   std::getline(ifs, line);
@@ -366,13 +366,17 @@ int Convert::loadChannelConfigFile(std::string channel_config_content){
     std::getline(ifs, line);
     std::vector<std::string> ChannelLine;
     boost::split(ChannelLine, line, boost::is_any_of(","));
+    if (ChannelLine.size() != loop_num) {
+      std::cout << "channel config file format is wrong" << std::endl;
+      return -1;
+    }
     for(int j = 0; j < loop_num; j++){
       if(ChannelLine.size() == loop_num){
         m_PandarQTChannelConfig.m_vChannelConfigTable[j][i] = std::stoi(ChannelLine[j].c_str());
         // printf("%d  %d  \n",i, m_PandarQTChannelConfig.m_vChannelConfigTable[j][i]);
       }
       else{
-        std::cout << "loop num is not equal to the first channel line";
+        std::cout << "loop num is not equal to the first channel line" << std::endl;
         return -1;
       }
 
@@ -772,7 +776,7 @@ void Convert::init() {
                   PANDAR128_AZIMUTH_SIZE * header->u8BlockNum + 
                   PANDAR128_CRC_SIZE + 
                   (header->hasFunctionSafety()? PANDAR128_FUNCTION_SAFETY_SIZE : 0));
-            m_iWorkMode = tail->nShutdownFlag & 0x03;
+            m_iWorkMode = tail->getOperationMode();
             m_iReturnMode = tail->nReturnMode;
             drv.setUdpVersion(m_u8UdpVersionMajor, m_u8UdpVersionMinor);
             lidarmotorspeed = tail->nMotorSpeed;
@@ -953,7 +957,7 @@ int Convert::checkLiadaMode() {
                   PANDAR128_AZIMUTH_SIZE * header->u8BlockNum + 
                   PANDAR128_CRC_SIZE + 
                   (header->hasFunctionSafety()? PANDAR128_FUNCTION_SAFETY_SIZE : 0));
-                lidarworkmode = tail->nShutdownFlag & 0x03;
+                lidarworkmode = tail->getOperationMode();
                 lidarreturnmode = tail->nReturnMode;
                 lidarmotorspeed = tail->nMotorSpeed;
                 laserNum = header->u8LaserNum;
@@ -1221,9 +1225,8 @@ void Convert::calcPointXYZIT(pandar_msgs::PandarPacket &packet, int cursor) {
       // ROS_WARN("#####block.fAzimuth[%u]",u16Azimuth);
       index += PANDAR128_AZIMUTH_SIZE;
 
-      int mode = tail->nShutdownFlag & 0x03;
-      int state = (tail->nShutdownFlag & 0xF0) >> 4;
-
+      int mode = tail->getOperationMode();
+      int state = tail->getAngleState(blockid);
       for (int i = 0; i < header->u8LaserNum; i++) {
         /* for all the units in a block */
         uint16_t u16Distance = *(uint16_t*)(&packet.data[0] + index);
@@ -1343,7 +1346,7 @@ void Convert::calcQT128PointXYZIT(pandar_msgs::PandarPacket &packet, int cursor)
   int index = 0;
   index += PANDAR128_HEAD_SIZE;
   for (int blockid = 0; blockid < header->u8BlockNum; blockid++) {
-    int loopIndex = blockid;
+    int loopIndex = (tail->nModeFlag + (blockid / ((tail->nReturnMode < 0x39) ? 1 : 2)) + 1) % header->u8BlockNum;
     if((isSelfDefine && m_PandarQTChannelConfig.m_bIsChannelConfigObtained)){
         loopIndex = (tail->nModeFlag + (blockid / ((tail->nReturnMode < 0x39) ? 1 : 2)) + 1) % m_PandarQTChannelConfig.m_vChannelConfigTable.size();
     }

pandar_pointcloud/src/conversions/convert.h

@@ -235,6 +235,10 @@ typedef struct __attribute__((__packed__)) Pandar128TailVersion14_s {
   uint32_t nTimestamp;
   uint8_t nFactoryInfo;
   uint32_t nSeqNum;
+  inline uint8_t getAngleState(int blockIndex) const {
+    return (nAzimuthFlag >> (2 * (7 - blockIndex))) & 0x03;
+  }
+  inline uint8_t getOperationMode() const { return nShutdownFlag & 0x0f; }
 } Pandar128TailVersion14;
 
 typedef struct __attribute__((__packed__)) PandarQT128Tail_s {
@@ -250,6 +254,10 @@ typedef struct __attribute__((__packed__)) PandarQT128Tail_s {
   uint32_t nTimestamp;
   uint8_t nFactoryInfo;
   uint32_t nSeqNum;
+  inline uint8_t getAngleState(int blockIndex) const {
+    return (nAzimuthFlag >> (2 * (7 - blockIndex))) & 0x03;
+  }
+  inline uint8_t getOperationMode() const { return nShutdownFlag & 0x0f; }
 } PandarQT128Tail;
 
 typedef struct __attribute__((__packed__)) Pandar128PacketVersion13_t {
@@ -323,6 +331,13 @@ typedef struct PacketsBuffer_s {
         lastOverflowed = false;
         ROS_WARN("buffer recovered");
       }
+      if(((m_iterPush > m_iterTaskEnd) && (m_iterPush - m_iterTaskEnd) > 4 * m_stepSize) ||
+      ((m_iterPush < m_iterTaskBegin) && (m_iterTaskBegin - m_iterPush) < CIRCLE - 4 * m_stepSize)){
+
+        while((((m_iterPush > m_iterTaskEnd) && (m_iterPush - m_iterTaskEnd) > 4 * m_stepSize) ||
+        ((m_iterPush < m_iterTaskBegin) && (m_iterTaskBegin - m_iterPush) < CIRCLE - 4 * m_stepSize)))
+          usleep(1000);
+      }
       *(m_iterPush++) = pkt;
       return 1;
     }