Merge pull request #52 from RoboSense-LiDAR/dev_opt

[MX]:[Decoder]:Update msop protocol

CHANGELOG.md

@@ -1,5 +1,13 @@
 # CHANGLOG
 
+## v1.5.16 2024-08-27
+### Added
+- Support sn parsing of RSMX.
+### Fixed
+- Update msop protocol of RSMX.
+- Fix compilation bug in epoll socket.
+- Fix compilation warning for unit tests.
+
 ## v1.5.15 2024-08-07
 ### Added
 - Support RSM3.
@@ -8,7 +16,7 @@
 ### Added
 - Support multiple lidars with different multicast addresses and the same port.
 ### Fixed
-- Fixed the bug that only one lidar was parsed correctly when multiple bp4.0 were used.
+- Fix the bug that only one lidar was parsed correctly when multiple bp4.0 were used.
 
 ## v1.5.13 2024-05-10
 ### Added

CMakeLists.txt

@@ -6,7 +6,7 @@ if(WIN32)
   cmake_policy(SET CMP0074 NEW) # CMake 3.12 required
 endif(WIN32)
 
-project(rs_driver VERSION 1.5.15)
+project(rs_driver VERSION 1.5.16)
 
 #========================
 #  Project setup

doc/src_intro/rs_driver_intro_CN.md

@@ -1192,7 +1192,7 @@ Block间的角度差 = 360 / 每帧Block数
 | RX             |    0.01473 | 光心相对于物理中心的X坐标  |
 | RY             |    0.0085  | 光心相对于物理中心的Y坐标  |
 | RZ             |    0.09427 | 光心相对于物理中心的Z坐标 |
-| BLOCK_DURATION |    55.52   | Block的持续时间，单位纳秒 |
+| BLOCK_DURATION |    55.52   | Block的持续时间，单位微秒 |
 | CHAN_TSS[]     |    -       | 从发射时间列表得到 |
 | CHAN_AZIS[]    |    -       | 从发射时间列表得到 |
 

src/rs_driver/driver/decoder/decoder_RSMX.hpp

@@ -65,9 +65,11 @@ typedef struct
 {
   uint8_t id[4];
   uint16_t pkt_seq;
-  uint8_t protocol_version;
-  uint8_t device_mode;
+  uint16_t protocol_version;
+  uint8_t return_mode;
+  uint8_t time_mode;
   RSTimestampUTC timestamp;
+  uint8_t reserved[10];
   uint8_t lidar_type;
   uint8_t temperature;
 } RSMXMsopHeader;
@@ -96,10 +98,19 @@ typedef struct
 {
   RSMXMsopHeader header;
   RSMXBlock blocks[50];
+  uint8_t reserved[16];
   uint8_t crc32[4];
   uint8_t rolling_counter[2];
 } RSMXMsopPkt;
 
+typedef struct
+{
+  uint8_t id[8];
+  uint8_t reserved1[30];
+  RSSN sn;
+  uint8_t reserved2[212];
+} RSMXDifopPkt;
+
 #pragma pack(pop)
 
 template <typename T_PointCloud>
@@ -129,7 +140,7 @@ inline RSDecoderConstParam& DecoderRSMX<T_PointCloud>::getConstParam()
 {
   static RSDecoderConstParam param = 
   {
-     1376// msop len
+     1404// msop len
       , 256 // difop len
       , 4 // msop id len
       , 8 // difop id len
@@ -175,6 +186,14 @@ template <typename T_PointCloud>
 inline void DecoderRSMX<T_PointCloud>::decodeDifopPkt(const uint8_t* packet, size_t size)
 {
 
+  #ifdef ENABLE_DIFOP_PARSE
+    const RSMXDifopPkt& pkt = *(RSMXDifopPkt*)packet;
+    // device info
+    memcpy (this->device_info_.sn, pkt.sn.num, 4);
+    this->device_info_.state = true;
+    // device status
+    this->device_status_.state = false;
+  #endif
 }
 
 template <typename T_PointCloud>
@@ -203,16 +222,17 @@ inline bool DecoderRSMX<T_PointCloud>::decodeMsopPkt(const uint8_t* packet, size
     }
   } 
 
-
+  
   uint16_t pkt_seq = ntohs(pkt.header.pkt_seq);
   if (split_strategy_.newPacket(pkt_seq))
   {
     this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs());
     this->first_point_ts_ = pkt_ts;
     ret = true;
   }
+
   uint8_t loop_time;
-  if((pkt.header.device_mode & 0xF) == 0)
+  if(pkt.header.return_mode == 0x0)
   { 
     loop_time = 2;  // dual return
   }else
@@ -243,6 +263,7 @@ inline bool DecoderRSMX<T_PointCloud>::decodeMsopPkt(const uint8_t* packet, size
           distance = ntohs(channel.radius_sd) * this->const_param_.DISTANCE_RES;
           intensity = channel.intensity_sd;
         }
+
           if (this->distance_section_.in(distance))
         {
           uint16_t vector_x = RS_SWAP_INT16(channel.x);

src/rs_driver/driver/input/unix/input_sock_epoll.hpp

@@ -179,11 +179,7 @@ inline int InputSock::createSocket(uint16_t port, const std::string& hostIp, con
     goto failOption;
   }
 
-   if (hostIp != "0.0.0.0" && grpIp != "0.0.0.0")
-  {
-    inet_pton(AF_INET, grpIp.c_str(), &(host_addr.sin_addr));
-  }
-
+
   struct sockaddr_in host_addr;
   memset(&host_addr, 0, sizeof(host_addr));
   host_addr.sin_family = AF_INET;
@@ -194,6 +190,12 @@ inline int InputSock::createSocket(uint16_t port, const std::string& hostIp, con
     inet_pton(AF_INET, hostIp.c_str(), &(host_addr.sin_addr));
   }
 
+  if (hostIp != "0.0.0.0" && grpIp != "0.0.0.0")
+  {
+    inet_pton(AF_INET, grpIp.c_str(), &(host_addr.sin_addr));
+  }
+
+
   ret = bind(fd, (struct sockaddr*)&host_addr, sizeof(host_addr));
   if (ret < 0)
   {

src/rs_driver/macro/version.hpp

@@ -1,3 +1,3 @@
 #define RSLIDAR_VERSION_MAJOR 1
 #define RSLIDAR_VERSION_MINOR 5
-#define RSLIDAR_VERSION_PATCH 15
+#define RSLIDAR_VERSION_PATCH 16

test/decoder_test.cpp

@@ -13,16 +13,21 @@ typedef PointCloudT<PointT> PointCloud;
 #pragma pack(push, 1)
 struct MyMsopPkt
 {
-  uint8_t id[8];
+  uint8_t id[8]{0};
 };
 
 struct MyDifopPkt
 {
   uint8_t id[8];
   uint16_t rpm;
+
   RSFOV fov;
   RSCalibrationAngle vert_angle_cali[2];
   RSCalibrationAngle horiz_angle_cali[2];
+  RSSN sn;
+  RSEthNetV2 eth;
+  RSVersionV2 version;
+  RSStatusV1 status;
 };
 #pragma pack(pop)
 
@@ -282,6 +287,10 @@ TEST(TestDecoder, processMsopPkt)
   memcpy (pkt.id, id, 8);
   errCode = ERRCODE_SUCCESS;
   decoder.processMsopPkt((const uint8_t*)&pkt, sizeof(pkt));
+#ifdef ENABLE_CRC32_CHECK
+  ASSERT_EQ(errCode, ERRCODE_WRONGCRC32);
+#else
   ASSERT_EQ(errCode, ERRCODE_SUCCESS);
+#endif
 }