Merge pull request #47 from RoboSense-LiDAR/dev_opt

[Input,Decoder]:[feat,fix]:support multiple lidars with same port, fi…

CHANGELOG.md

@@ -2,6 +2,12 @@
 
 ## Unreleased
 
+## v1.5.14 2024-07-15
+### 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.
+
 ## v1.5.13 2024-05-10
 ### Added
 - Support RSMX.

src/rs_driver/driver/decoder/decoder_RSBP.hpp

@@ -103,7 +103,9 @@ class DecoderRSBP : public DecoderMech<T_PointCloud>
 
   template <typename T_BlockIterator>
   bool internDecodeMsopPkt(const uint8_t* pkt, size_t size);
-  bool reversal_;
+  bool reversal_{false};
+  bool isBpV4_{false};
+  bool isFirstPkt_{true};
 };
 
 template <typename T_PointCloud>
@@ -208,16 +210,15 @@ inline bool DecoderRSBP<T_PointCloud>::internDecodeMsopPkt(const uint8_t* packet
 {
   const RSBPMsopPkt& pkt = *(const RSBPMsopPkt*)(packet);
   bool ret = false;
-  static bool isBpV4 = false;
-  static bool isFirstPkt = true;
+
   this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES;
   this->is_get_temperature_ = true;
-  if(isFirstPkt)
+  if(isFirstPkt_)
   {
-    isFirstPkt = false;
-      if ((pkt.header.lidar_type == 0x03) && (pkt.header.lidar_model == 0x04)) 
+    isFirstPkt_ = false;
+    if ((pkt.header.lidar_type == 0x03) && (pkt.header.lidar_model == 0x04)) 
     {
-      isBpV4 = true;
+      isBpV4_ = true;
       this->const_param_.DISTANCE_RES = 0.0025f;
       this->mech_const_param_.RX = 0.01619f;
       this->mech_const_param_.RY = 0.0085f;
@@ -245,7 +246,7 @@ inline bool DecoderRSBP<T_PointCloud>::internDecodeMsopPkt(const uint8_t* packet
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {
-    if (isBpV4) 
+    if (isBpV4_) 
       pkt_ts = parseTimeUTCWithUs ((RSTimestampUTC*)&pkt.header.timestamp) * 1e-6;
     else
       pkt_ts = parseTimeYMD (&pkt.header.timestamp) * 1e-6;
@@ -259,7 +260,7 @@ inline bool DecoderRSBP<T_PointCloud>::internDecodeMsopPkt(const uint8_t* packet
 
     if (this->write_pkt_ts_)
     {
-      if (isBpV4) 
+      if (isBpV4_) 
         createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp);
       else
         createTimeYMD (ts, (RSTimestampYMD*)&pkt.header.timestamp);

src/rs_driver/driver/decoder/decoder_RSMX.hpp

@@ -211,7 +211,6 @@ inline bool DecoderRSMX<T_PointCloud>::decodeMsopPkt(const uint8_t* packet, size
     this->first_point_ts_ = pkt_ts;
     ret = true;
   }
-
   uint8_t loop_time;
   if((pkt.header.device_mode & 0xF) == 0)
   { 
@@ -226,11 +225,10 @@ inline bool DecoderRSMX<T_PointCloud>::decodeMsopPkt(const uint8_t* packet, size
     const RSMXBlock& block = pkt.blocks[blk];
 
     double point_time = pkt_ts + block.time_offset * 1e-6;
-
     for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++)
     {
       const RSMXChannel& channel = block.channel[chan];
-
+      
       float distance;
       uint8_t intensity;
 

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

@@ -179,6 +179,11 @@ 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;

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

@@ -161,6 +161,10 @@ 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/driver/input/win/input_sock_select.hpp

@@ -157,6 +157,11 @@ 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;