pandora: sync code with the latest version of https://github.com/HesaiTechnology/Pandora_ros

pandora/pandora_driver/launch/pandora_driver_apollo.launch

@@ -8,8 +8,8 @@
 	<arg name="timezone"  default="8"/>
 	<arg name="frame_id"  default="velodyne64"/>
 	<arg name="lidar_topic"  default="/apollo/sensor/velodyne64/PointCloud2"/>
-	<arg name="camera0_topic"  default="/apollo/sensor/pandora/camera/front_color"/>
-	<arg name="camera1_topic"  default="/apollo/sensor/pandora/camera/front_gray"/>
+	<arg name="camera0_topic"  default="/apollo/sensor/camera/traffic/image_long"/>
+	<arg name="camera1_topic"  default="/apollo/sensor/camera/traffic/image_short"/>
 	<arg name="camera2_topic"  default="/apollo/sensor/pandora/camera/right_gray"/>
 	<arg name="camera3_topic"  default="/apollo/sensor/pandora/camera/back_gray"/>
 	<arg name="camera4_topic"  default="/apollo/sensor/pandora/camera/left_gray"/>

pandora/pandora_driver/src/Pandar40P/CMakeLists.txt

@@ -5,7 +5,7 @@ project(Pandar40P)
 find_package( Boost REQUIRED )
 find_package(PCL REQUIRED COMPONENTS common)
 
-set (CMAKE_CXX_FLAGS "-fPIC -std=c++11")
+set (CMAKE_CXX_FLAGS "-fPIC --std=c++11")
 
 
 include_directories(

pandora/pandora_driver/src/Pandar40P/include/pandar40p/pandar40p.h

@@ -17,14 +17,15 @@
 #ifndef INCLUDE_PANDAR40P_H_
 #define INCLUDE_PANDAR40P_H_
 
-#include <boost/function.hpp>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 #include <pthread.h>
 #include <semaphore.h>
 
 #include <string>
 
+#include <boost/function.hpp>
+
 #include "pandar40p/point_types.h"
 
 namespace apollo {
@@ -45,15 +46,14 @@ class Pandar40P {
    *        start_angle       The start angle of every point cloud ,
    *                          should be <real angle> * 100.
    */
-  Pandar40P(const std::string &device_ip,
-            uint16_t lidar_port, uint16_t gps_port,
+  Pandar40P(const std::string &device_ip, uint16_t lidar_port, uint16_t gps_port,
             boost::function<void(boost::shared_ptr<PPointCloud>, double)>
                 pcl_callback,
             boost::function<void(double)> gps_callback, uint16_t start_angle,
             int tz, std::string frame_id);
 
   /**
-   * @brief destructor
+   * @brief deconstructor
    */
   ~Pandar40P();
 

pandora/pandora_driver/src/Pandar40P/src/input.cc

@@ -32,9 +32,7 @@ namespace drivers {
 namespace hesai {
 
 Input::Input(uint16_t port, uint16_t gpsPort) {
-  socketForGPS = -1;
   socketForLidar = -1;
-
   socketForLidar = socket(PF_INET, SOCK_DGRAM, 0);
   if (socketForLidar == -1) {
     perror("socket");  // TODO(Philip.Pi): perror errno.
@@ -63,6 +61,7 @@ Input::Input(uint16_t port, uint16_t gpsPort) {
     return;
   }
   // gps socket
+  socketForGPS = -1;
   socketForGPS = socket(PF_INET, SOCK_DGRAM, 0);
   if (socketForGPS == -1) {
     perror("socket");  // TODO(Philip.Pi): perror errno.
@@ -128,7 +127,7 @@ int Input::getPacket(PandarPacket *pkt) {
   }
 
   senderAddressLen = sizeof(senderAddress);
-  ssize_t nbytes = 0;
+  ssize_t nbytes;
   for (int i = 0; i != socketNumber; ++i) {
     if (fds[i].revents & POLLIN) {
       nbytes = recvfrom(fds[i].fd, &pkt->data[0], ETHERNET_MTU, 0,

pandora/pandora_driver/src/Pandar40P/src/input.h

@@ -18,7 +18,6 @@
 #define SRC_INPUT_H_
 
 #include <netinet/in.h>
-#include <pcap.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <string>
@@ -39,9 +38,8 @@ class Input {
  public:
   Input(uint16_t port, uint16_t gpsPort);
   ~Input();
-  Input(std::string filePath, int type);  // not implemented
+  Input(std::string filePath, int type);
   int getPacket(PandarPacket *pkt);
-  int getPacketFromPcap(PandarPacket *pkt);  // not implemented
 
  private:
   int socketForLidar;

pandora/pandora_driver/src/Pandar40P/src/pandar40p.cc

@@ -41,7 +41,7 @@ Pandar40P::Pandar40P(
 }
 
 /**
- * @brief destructor
+ * @brief deconstructor
  */
 Pandar40P::~Pandar40P() { delete internal_; }
 

pandora/pandora_driver/src/Pandar40P/src/pandar40p_internal.cc

@@ -14,7 +14,6 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include <algorithm>
 #include <sstream>
 
 #include "src/input.h"
@@ -38,7 +37,7 @@ static const float pandar40p_elev_angle_map[] = {
     -4.321, -4.657, -4.986,  -5.311,  -5.647,  -5.974, -6.957,  -7.934,
     -8.908, -9.871, -10.826, -11.772, -12.705, -13.63, -14.543, -15.444};
 
-// Line 40 Lidar azimuth Horizontal offset ,  Line 1 - Line 40
+// Line 40 Lidar azimuth Horizatal offset ,  Line 1 - Line 40
 static const float pandar40p_horizatal_azimuth_offset_map[] = {
     0.005,  0.006,  0.006,  0.006,  -2.479, -2.479, 2.491,  -4.953,
     -2.479, 2.492,  -4.953, -2.479, 2.492,  -4.953, 0.007,  2.491,
@@ -47,7 +46,7 @@ static const float pandar40p_horizatal_azimuth_offset_map[] = {
     0.004,  0.004,  0.003,  0.003,  -2.466, -2.463, -2.46,  -2.457};
 
 Pandar40P_Internal::Pandar40P_Internal(
-    const std::string &device_ip, uint16_t lidar_port, uint16_t gps_port,
+    std::string device_ip, uint16_t lidar_port, uint16_t gps_port,
     boost::function<void(boost::shared_ptr<PPointCloud>, double)> pcl_callback,
     boost::function<void(double)> gps_callback, uint16_t start_angle, int tz,
     std::string frame_id) {
@@ -60,7 +59,7 @@ Pandar40P_Internal::Pandar40P_Internal(
   enable_lidar_recv_thr_ = false;
   enable_lidar_process_thr_ = false;
 
-  start_angle_ = start_angle > 36000 ? 0 : start_angle;
+  start_angle_ = start_angle;
 
   for (uint16_t rotIndex = 0; rotIndex < ROTATION_MAX_UNITS; ++rotIndex) {
     float rotation = degreeToRadian(0.01 * static_cast<double>(rotIndex));
@@ -73,6 +72,8 @@ Pandar40P_Internal::Pandar40P_Internal(
   pcl_callback_ = pcl_callback;
   gps_callback_ = gps_callback;
 
+  last_azimuth_ = 0;
+
   // init the block time offset, us
   blockOffset_[9] = 55.1f * 0.0 + 45.18f;
   blockOffset_[8] = 55.1f * 1.0 + 45.18f;
@@ -148,7 +149,7 @@ Pandar40P_Internal::~Pandar40P_Internal() {
  * @brief load the correction file
  * @param file The path of correction file
  */
-int Pandar40P_Internal::LoadCorrectionFile(const std::string &correction_content) {  // NOLINT
+int Pandar40P_Internal::LoadCorrectionFile(std::string correction_content) {
   std::istringstream ifs(correction_content);
 
   std::string line;
@@ -193,7 +194,7 @@ int Pandar40P_Internal::LoadCorrectionFile(const std::string &correction_content
 }
 
 /**
- * @brief reset the start angle
+ * @brief load the correction file
  * @param angle The start angle
  */
 void Pandar40P_Internal::ResetStartAngle(uint16_t start_angle) {
@@ -251,9 +252,9 @@ void Pandar40P_Internal::RecvTask() {
 }
 
 void Pandar40P_Internal::ProcessLiarPacket() {
+  double lastTimestamp = 0.0f;
   struct timespec ts;
   int ret = 0;
-  static int packetIndex = 0;
 
   boost::shared_ptr<PPointCloud> outMsg(new PPointCloud());
 
@@ -279,31 +280,29 @@ void Pandar40P_Internal::ProcessLiarPacket() {
         continue;
       }
 
-      packetIndex++;
-
       for (int i = 0; i < BLOCKS_PER_PACKET; ++i) {
-        /* ready a round ? */
-        uint16_t current_azimuth = pkt.blocks[i].azimuth;
-        int limit_degree = RATE_PER_PACKET * 100/2;
-        int gap = std::min(std::abs(current_azimuth - start_angle_),
-                36000 - std::abs(current_azimuth - start_angle_));
-
-        // at lease 150 packets, no more than 182
-        if ((packetIndex > PACKETS_PER_ROUND * 5/6 && gap <= limit_degree) ||
-                packetIndex > PACKETS_PER_ROUND + 1) {
-          // ok
-          if (pcl_callback_ && outMsg->points.size() > 0) {
-            // std::cout << std::fixed << std::setprecision(18)
-            //    << "Size: " << packetIndex << ", current: " << current_azimuth
-            //    << ", timestamp_: " << timestamp_ << std::endl;
-            pcl_callback_(outMsg, timestamp_);
-            outMsg.reset(new PPointCloud());
-            // reset
-            packetIndex = 0; timestamp_ = 0;
+        int azimuthGap = 0; /* To do */
+        if(last_azimuth_ > pkt.blocks[i].azimuth) {
+          azimuthGap = static_cast<int>(pkt.blocks[i].azimuth) + (36000 - static_cast<int>(last_azimuth_));
+        } else {
+          azimuthGap = static_cast<int>(pkt.blocks[i].azimuth) - static_cast<int>(last_azimuth_);
+        }
+
+        if (last_azimuth_ != pkt.blocks[i].azimuth && 
+                      azimuthGap < 600 /* 6 degree*/) {
+          /* for all the blocks */
+          if ((last_azimuth_ > pkt.blocks[i].azimuth &&
+               start_angle_ <= pkt.blocks[i].azimuth) ||
+              (last_azimuth_ < start_angle_ &&
+               start_angle_ <= pkt.blocks[i].azimuth)) {
+            if (pcl_callback_ && outMsg->points.size() > 0) {
+              pcl_callback_(outMsg, outMsg->points[0].timestamp);
+              outMsg.reset(new PPointCloud());
+            }
           }
         }
-
         CalcPointXYZIT(&pkt, i, outMsg);
+        last_azimuth_ = pkt.blocks[i].azimuth;
       }
     } else {
       continue;
@@ -331,13 +330,13 @@ void Pandar40P_Internal::ProcessGps(const PandarGPS &gpsMsg) {
 
   // UTC's month start from 1, but mktime only accept month from 0.
   t.tm_mon = gpsMsg.month - 1;
-  // UTC's year only include 0 - 99 year , which indicates 2000 to 2099.
-  // and mktime's year start from 1900 which is 0, so we need to add 100 years.
+  // 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.
   t.tm_year = gpsMsg.year + 100;
   t.tm_isdst = 0;
 
   if (gps_callback_) {
-    gps_callback_(static_cast<double>(mktime(&t) + tz_second_) + 1);
+    gps_callback_(static_cast<double>(mktime(&t) + tz_second_));
   }
 }
 
@@ -393,7 +392,7 @@ int Pandar40P_Internal::ParseRawData(Pandar40PPacket *packet,
   // parse the UTC Time.
 
   // 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 to add 100 years.
+  // and mktime's year start from 1900 which is 0. so we need add 100 year.
   packet->t.tm_year = (buf[index + 0] & 0xff) + 100;
   // UTC's month start from 1, but mktime only accept month from 0.
   packet->t.tm_mon = (buf[index + 1] & 0xff) - 1;
@@ -458,7 +457,7 @@ void Pandar40P_Internal::CalcPointXYZIT(Pandar40PPacket *pkt, int blockid,
   Pandar40PBlock *block = &pkt->blocks[blockid];
 
   double unix_second =
-      static_cast<double>(mktime(&pkt->t) + 1 + tz_second_);  // 1 second offset
+      static_cast<double>(mktime(&pkt->t) + tz_second_);
 
   for (int i = 0; i < LASER_COUNT; ++i) {
     /* for all the units in a block */
@@ -499,7 +498,7 @@ void Pandar40P_Internal::CalcPointXYZIT(Pandar40PPacket *pkt, int blockid,
       // dual return, block 0&1 (2&3 , 4*5 ...)'s timestamp is the same.
       point.timestamp =
           point.timestamp - (static_cast<double>(blockOffset_[blockid / 2] +
-                                                 laserOffset_[i / 2]) /
+                                                 laserOffset_[i]) /
                              1000000.0f);
     } else {
       point.timestamp =
@@ -509,11 +508,6 @@ void Pandar40P_Internal::CalcPointXYZIT(Pandar40PPacket *pkt, int blockid,
     }
 
     point.ring = i;
-    // get smallest timestamp
-    if ((timestamp_ > 0 && timestamp_ < point.timestamp)
-            || timestamp_ <= 0) {
-      timestamp_ = point.timestamp;
-    }
 
     cld->push_back(point);
   }

pandora/pandora_driver/src/Pandar40P/src/pandar40p_internal.h

@@ -17,7 +17,6 @@
 #ifndef SRC_PANDAR40P_INTERNAL_H_
 #define SRC_PANDAR40P_INTERNAL_H_
 
-#include <boost/function.hpp>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 #include <pthread.h>
@@ -26,11 +25,11 @@
 #include <list>
 #include <string>
 
+#include <boost/function.hpp>
+
 #include "pandar40p/point_types.h"
 #include "src/input.h"
 
-#define RATE_PER_PACKET (2)
-#define PACKETS_PER_ROUND (360 / RATE_PER_PACKET)
 #define SOB_ANGLE_SIZE (4)
 #define RAW_MEASURE_SIZE (3)
 #define LASER_COUNT (40)
@@ -43,7 +42,7 @@
 #define REVOLUTION_SIZE (2)
 #define INFO_SIZE                                                  \
   (TIMESTAMP_SIZE + FACTORY_INFO_SIZE + ECHO_SIZE + RESERVE_SIZE + \
-  REVOLUTION_SIZE)
+   REVOLUTION_SIZE)
 #define UTC_TIME (6)
 #define PACKET_SIZE (BLOCK_SIZE * BLOCKS_PER_PACKET + INFO_SIZE + UTC_TIME)
 #define LASER_RETURN_TO_DISTANCE_RATE (0.004)
@@ -112,7 +111,7 @@ class Pandar40P_Internal {
    *        type       				The device type
    */
   Pandar40P_Internal(
-      const std::string &device_ip, uint16_t lidar_port, uint16_t gps_port,
+      std::string device_ip, uint16_t lidar_port, uint16_t gps_port,
       boost::function<void(boost::shared_ptr<PPointCloud>, double)>
           pcl_callback,
       boost::function<void(double)> gps_callback, uint16_t start_angle, int tz,
@@ -122,10 +121,10 @@ class Pandar40P_Internal {
    * @brief load the correction file
    * @param correction The path of correction file
    */
-  int LoadCorrectionFile(const std::string &correction);
+  int LoadCorrectionFile(std::string correction);
 
   /**
-   * @brief reset the start angle.
+   * @brief load the correction file
    * @param angle The start angle
    */
   void ResetStartAngle(uint16_t start_angle);
@@ -151,8 +150,7 @@ class Pandar40P_Internal {
   boost::thread *lidar_process_thr_;
   bool enable_lidar_recv_thr_;
   bool enable_lidar_process_thr_;
-  int start_angle_ = 0;
-  double timestamp_ = 0;
+  int start_angle_;
 
   std::list<struct PandarPacket_s> lidar_packets_;
 
@@ -164,6 +162,8 @@ class Pandar40P_Internal {
   float sin_lookup_table_[ROTATION_MAX_UNITS];
   float cos_lookup_table_[ROTATION_MAX_UNITS];
 
+  uint16_t last_azimuth_;
+
   float elev_angle_map_[LASER_COUNT];
   float horizatal_azimuth_offset_map_[LASER_COUNT];