diff --git a/pandora/pandora_driver/launch/pandora_driver_apollo.launch b/pandora/pandora_driver/launch/pandora_driver_apollo.launch
index aecaed5..f212e91 100755
--- a/pandora/pandora_driver/launch/pandora_driver_apollo.launch
+++ b/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"/>
diff --git a/pandora/pandora_driver/src/Pandar40P/CMakeLists.txt b/pandora/pandora_driver/src/Pandar40P/CMakeLists.txt
index 1e0af55..f1e021f 100644
--- a/pandora/pandora_driver/src/Pandar40P/CMakeLists.txt
+++ b/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(
diff --git a/pandora/pandora_driver/src/Pandar40P/include/pandar40p/pandar40p.h b/pandora/pandora_driver/src/Pandar40P/include/pandar40p/pandar40p.h
index d936938..de2c689 100644
--- a/pandora/pandora_driver/src/Pandar40P/include/pandar40p/pandar40p.h
+++ b/pandora/pandora_driver/src/Pandar40P/include/pandar40p/pandar40p.h
@@ -17,7 +17,6 @@
 #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>
@@ -25,6 +24,8 @@
 
 #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();
 
diff --git a/pandora/pandora_driver/src/Pandar40P/src/input.cc b/pandora/pandora_driver/src/Pandar40P/src/input.cc
index 25a4d26..701ff44 100644
--- a/pandora/pandora_driver/src/Pandar40P/src/input.cc
+++ b/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,
diff --git a/pandora/pandora_driver/src/Pandar40P/src/input.h b/pandora/pandora_driver/src/Pandar40P/src/input.h
index dfcca66..bbdf596 100644
--- a/pandora/pandora_driver/src/Pandar40P/src/input.h
+++ b/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;
diff --git a/pandora/pandora_driver/src/Pandar40P/src/pandar40p.cc b/pandora/pandora_driver/src/Pandar40P/src/pandar40p.cc
index 5889b06..4182143 100644
--- a/pandora/pandora_driver/src/Pandar40P/src/pandar40p.cc
+++ b/pandora/pandora_driver/src/Pandar40P/src/pandar40p.cc
@@ -41,7 +41,7 @@ Pandar40P::Pandar40P(
 }
 
 /**
- * @brief destructor
+ * @brief deconstructor
  */
 Pandar40P::~Pandar40P() { delete internal_; }
 
diff --git a/pandora/pandora_driver/src/Pandar40P/src/pandar40p_internal.cc b/pandora/pandora_driver/src/Pandar40P/src/pandar40p_internal.cc
index 44e0aa7..3732ca6 100644
--- a/pandora/pandora_driver/src/Pandar40P/src/pandar40p_internal.cc
+++ b/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);
   }
diff --git a/pandora/pandora_driver/src/Pandar40P/src/pandar40p_internal.h b/pandora/pandora_driver/src/Pandar40P/src/pandar40p_internal.h
index 3fd46c9..ac68bfc 100644
--- a/pandora/pandora_driver/src/Pandar40P/src/pandar40p_internal.h
+++ b/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];
 
diff --git a/pandora/pandora_driver/src/Pandar40P/test/test.cc b/pandora/pandora_driver/src/Pandar40P/test/test.cc
index f4a9035..b92da5c 100644
--- a/pandora/pandora_driver/src/Pandar40P/test/test.cc
+++ b/pandora/pandora_driver/src/Pandar40P/test/test.cc
@@ -16,7 +16,7 @@
 
 #include "pandar40p/pandar40p.h"
 
-using apollo::drivers::hesai;
+using namespace apollo::drivers::hesai;
 
 FILE* lidarTimestampFile = fopen("lidar-timestamp.txt", "w");
 
@@ -27,10 +27,8 @@ void gpsCallback(int timestamp) {
   struct timeval ts;
   gettimeofday(&ts, NULL);
   gpsTimestamp = timestamp;
-  double sec = static_cast<double>(ts.tv_sec);
-  double usec = static_cast<double>(ts.tv_usec);
-  double t = static_cast<timestamp>(timestamp);
-  pandoraToSysTimeGap = tv_sec + (tv_usec / 1000000.0) - timestamp;
+  pandoraToSysTimeGap =
+      (double)ts.tv_sec + ((double)ts.tv_usec / 1000000.0) - (double)timestamp;
   printf("gps: %d, gap: %f\n", timestamp, pandoraToSysTimeGap);
 }
 
@@ -38,17 +36,16 @@ void lidarCallback(boost::shared_ptr<PPointCloud> cld, double timestamp) {
   struct timeval ts;
   gettimeofday(&ts, NULL);
   printf("lidar: %lf\n", timestamp);
-  double usec = static_cast<double>(ts.tv_usec);
   fprintf(lidarTimestampFile, "%d, %f,%f\n", gpsTimestamp, timestamp,
-          ts.tv_sec + tv_usec / 1000000 - pandoraToSysTimeGap - timestamp);
+          ts.tv_sec + (double)ts.tv_usec / 1000000 - pandoraToSysTimeGap -
+              timestamp);
 }
 
 int main(int argc, char** argv) {
   Pandar40P pandar40p(std::string("192.168.20.51"), 2368, 10110, lidarCallback,
-                      gpsCallback, 0);
+                      gpsCallback, 13500, 0, std::string("hesai40"));
   pandar40p.Start();
   while (true) {
     sleep(100);
   }
-}
-
+}
\ No newline at end of file
diff --git a/pandora/pandora_driver/src/main.cc b/pandora/pandora_driver/src/main.cc
index 4a446e8..65db2a8 100755
--- a/pandora/pandora_driver/src/main.cc
+++ b/pandora/pandora_driver/src/main.cc
@@ -146,11 +146,22 @@ class PandoraHesaiClient {
                       int pic_id, bool distortion) {
     sensor_msgs::ImagePtr imgMsg;
 
-    if (pic_id > 4 || pic_id < 0) {
-      ROS_INFO("picid wrong in getImageToPub");
+    if (pic_id > 1 || pic_id < 0) {
+      //ROS_INFO("picid wrong in getImageToPub");
       return;
     }
-    imgMsg = cv_bridge::CvImage(std_msgs::Header(), "rgb8", *matp).toImageMsg();
+
+    if (0 == pic_id)
+    {
+      imgMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", *matp).toImageMsg();
+    }
+    else
+    {
+      cv::Mat grayMat;
+      cv::cvtColor(*matp, grayMat, CV_RGB2GRAY);
+      imgMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", grayMat).toImageMsg();
+    }
+
     imgMsg->header.stamp = ros::Time(timestamp);
     imgPublishers[pic_id].publish(imgMsg);
   }
diff --git a/pandora/pandora_driver/src/pandora.cc b/pandora/pandora_driver/src/pandora.cc
index cd171a6..38a7398 100644
--- a/pandora/pandora_driver/src/pandora.cc
+++ b/pandora/pandora_driver/src/pandora.cc
@@ -29,8 +29,7 @@ namespace hesai {
 class Pandora_Internal {
  public:
   Pandora_Internal(
-      const std::string &device_ip,
-      const uint16_t lidar_port, const uint16_t gps_port,
+      std::string device_ip, const uint16_t lidar_port, const uint16_t gps_port,
       boost::function<void(boost::shared_ptr<PPointCloud>, double)>
           pcl_callback,
       boost::function<void(double)> gps_callback, uint16_t start_angle,
@@ -46,7 +45,7 @@ class Pandora_Internal {
   int GetCameraCalibration(CameraCalibration calibs[5]);
   int ResetCameraClibration();
   void GetCalibrationFromDevice();
-  int ParseCameraCalibration(const std::string &contents,
+  int ParseCameraCalibration(const std::string contents,
                              CameraCalibration calibs[5]);
   int GenerateCameraCalibration(const CameraCalibration calibs[5],
                                 std::string *contents);
@@ -69,8 +68,7 @@ class Pandora_Internal {
 };
 
 Pandora_Internal::Pandora_Internal(
-    const std::string &device_ip,
-    const uint16_t lidar_port, const uint16_t gps_port,
+    std::string device_ip, const uint16_t lidar_port, const uint16_t gps_port,
     boost::function<void(boost::shared_ptr<PPointCloud>, double)> pcl_callback,
     boost::function<void(double)> gps_callback, uint16_t start_angle,
     const uint16_t pandoraCameraPort,
@@ -117,7 +115,7 @@ Pandora_Internal::~Pandora_Internal() {
  * @brief load the correction file
  * @param file The path of correction file
  */
-int Pandora_Internal::LoadLidarCorrectionFile(const std::string &correction_content) {  // NOLINT
+int Pandora_Internal::LoadLidarCorrectionFile(const std::string &correction_content) {
   return pandar40p_->LoadCorrectionFile(correction_content);
 }
 
@@ -200,8 +198,8 @@ void Pandora_Internal::GetCalibrationFromDevice() {
          !got_camera_calibration_) {
     if (!got_lidar_calibration_) {
       // get lidar calibration.
-      char *buffer;
-      uint32_t len;
+      char *buffer = NULL;
+      uint32_t len = 0;
 
       ret = TcpCommandGetLidarCalibration(tcp_command_client_, &buffer, &len);
       if (ret == 0 && buffer) {
@@ -226,8 +224,8 @@ void Pandora_Internal::GetCalibrationFromDevice() {
 
     if (!got_camera_calibration_) {
       // get camera calibration.
-      char *buffer;
-      uint32_t len;
+      char *buffer = NULL;
+      uint32_t len = 0;
 
       ret = TcpCommandGetCalibration(tcp_command_client_, &buffer, &len);
       if (ret == 0 && buffer) {
@@ -262,7 +260,7 @@ void Pandora_Internal::GetCalibrationFromDevice() {
   }
 }
 
-int Pandora_Internal::ParseCameraCalibration(const std::string &contents,
+int Pandora_Internal::ParseCameraCalibration(const std::string contents,
                                              CameraCalibration calibs[5]) {
   std::cout << "Parse Camera Calibration..." << std::endl;
   if (contents.empty()) {
@@ -392,8 +390,7 @@ Pandora Part
  *        cameraCallback    the call back for camera data
  */
 Pandora::Pandora(
-    const std::string &device_ip,
-    const uint16_t lidar_port, const uint16_t gps_port,
+    const std::string &device_ip, const uint16_t lidar_port, const uint16_t gps_port,
     boost::function<void(boost::shared_ptr<PPointCloud>, double)> pcl_callback,
     boost::function<void(double)> gps_callback, uint16_t start_angle,
     const uint16_t pandoraCameraPort,
@@ -407,7 +404,7 @@ Pandora::Pandora(
 }
 
 /**
- * @brief destructor
+ * @brief deconstructor
  */
 Pandora::~Pandora() { delete internal_; }
 
diff --git a/pandora/pandora_driver/src/pandora_camera.cc b/pandora/pandora_driver/src/pandora_camera.cc
index f229965..bec0046 100644
--- a/pandora/pandora_driver/src/pandora_camera.cc
+++ b/pandora/pandora_driver/src/pandora_camera.cc
@@ -38,15 +38,20 @@ static int CameraClientCallback(void *handle, int cmd, void *param,
 }
 
 PandoraCamera::PandoraCamera(
-    const std::string &device_ip, const uint16_t pandoraCameraPort,
+    std::string device_ip, const uint16_t pandoraCameraPort,
     boost::function<void(boost::shared_ptr<cv::Mat> matp, double timestamp,
                          int picid, bool distortion)>
         camera_callback,
     boost::function<void(bool connected)> connectionChanged, int tz) {
   ip_ = device_ip;
-  sem_init(&pic_sem_, 0, 0);
-  pthread_mutex_init(&pic_lock_, NULL);
-  process_pic_thread_ = NULL;
+
+  for (int i = 0; i < CAMERA_NUM; ++i) {
+    /* init pic lock / sem  */
+    sem_init(&pic_sem_[i], 0, 0);
+    pthread_mutex_init(&pic_lock_[i], NULL);
+    process_pic_thread_[i] = NULL;
+  }
+  
   continue_process_pic_ = false;
   need_remap_ = false;
   camera_port_ = pandoraCameraPort;
@@ -58,8 +63,11 @@ PandoraCamera::PandoraCamera(
 
 PandoraCamera::~PandoraCamera() {
   Stop();
-  sem_destroy(&pic_sem_);
-  pthread_mutex_destroy(&pic_lock_);
+  for (int i = 0; i < CAMERA_NUM; ++i)
+  {
+    sem_destroy(&pic_sem_[i]);
+    pthread_mutex_destroy(&pic_lock_[i]);
+  }
 }
 
 int PandoraCamera::Start() {
@@ -71,12 +79,16 @@ int PandoraCamera::Start() {
     continue_process_pic_ = false;
     return -1;
   }
-  process_pic_thread_ =
-      new boost::thread(boost::bind(&PandoraCamera::processPic, this));
-  if (!process_pic_thread_) {
-    continue_process_pic_ = false;
-    PandoraClientDestroy(pandora_client_);
-    return -1;
+
+  for (int i = 0; i < CAMERA_NUM; ++i) {
+    process_pic_thread_[i] =
+        new boost::thread(boost::bind(&PandoraCamera::processPic, this , i));
+    if (!process_pic_thread_[i]) {
+      continue_process_pic_ = false;
+      PandoraClientDestroy(pandora_client_);
+      pandora_client_ = NULL;
+      return -1;
+    }
   }
   return 0;
 }
@@ -88,21 +100,24 @@ void PandoraCamera::Stop() {
   }
   pandora_client_ = NULL;
 
-  if (process_pic_thread_) {
-    process_pic_thread_->join();
-    delete process_pic_thread_;
+  for (int i = 0; i < CAMERA_NUM; ++i) {
+    if (process_pic_thread_[i]) {
+      process_pic_thread_[i]->join();
+      delete process_pic_thread_[i];
+    }
+    process_pic_thread_[i] = NULL;
   }
-  process_pic_thread_ = NULL;
+  
 }
 
 void PandoraCamera::pushPicture(PandoraPic *pic) {
-  pthread_mutex_lock(&pic_lock_);
-  pic_list_.push_back(pic);
-  pthread_mutex_unlock(&pic_lock_);
-  sem_post(&pic_sem_);
+  pthread_mutex_lock(&pic_lock_[pic->header.pic_id]);
+  pic_list_[pic->header.pic_id].push_back(pic);
+  pthread_mutex_unlock(&pic_lock_[pic->header.pic_id]);
+  sem_post(&pic_sem_[pic->header.pic_id]);
 }
 
-void PandoraCamera::processPic() {
+void PandoraCamera::processPic(int pic_id) {
   while (continue_process_pic_) {
     struct timespec ts;
     if (clock_gettime(CLOCK_REALTIME, &ts) == -1) {
@@ -110,14 +125,14 @@ void PandoraCamera::processPic() {
     }
 
     ts.tv_sec += 1;
-    if (sem_timedwait(&pic_sem_, &ts) == -1) {
+    if (sem_timedwait(&pic_sem_[pic_id], &ts) == -1) {
       continue;
     }
 
-    pthread_mutex_lock(&pic_lock_);
-    PandoraPic *pic = pic_list_.front();
-    pic_list_.pop_front();
-    pthread_mutex_unlock(&pic_lock_);
+    pthread_mutex_lock(&pic_lock_[pic_id]);
+    PandoraPic *pic = pic_list_[pic_id].front();
+    pic_list_[pic_id].pop_front();
+    pthread_mutex_unlock(&pic_lock_[pic_id]);
 
     if (pic == NULL) {
       printf("pic is NULL\n");
@@ -136,16 +151,13 @@ void PandoraCamera::processPic() {
     boost::shared_ptr<cv::Mat> cvMatPic(new cv::Mat());
     switch (pic->header.pic_id) {
       case 0:
-        yuv422ToCvmat(cvMatPic, pic->yuv, pic->header.width, pic->header.height,
-                      8);
-        if (need_remap_)
-          remap(cvMatPic->clone(), *cvMatPic, mapx_[pic->header.pic_id],
-                mapy_[pic->header.pic_id], CV_INTER_LINEAR);
-        break;
+        //yuv422ToCvmat(cvMatPic, pic->yuv, pic->header.width, pic->header.height,
+        //              8);
+        //if (need_remap_)
+        //  remap(cvMatPic->clone(), *cvMatPic, mapx_[pic->header.pic_id],
+        //        mapy_[pic->header.pic_id], CV_INTER_LINEAR);
+        //break;
       case 1:
-      case 2:
-      case 3:
-      case 4:
         uint8_t *bmp;
         uint32_t bmpSize;
         decompressJpeg(static_cast<uint8_t *>(pic->yuv), pic->header.len, &bmp,
@@ -157,18 +169,22 @@ void PandoraCamera::processPic() {
                 mapy_[pic->header.pic_id], CV_INTER_LINEAR);
 
         free(bmp);
+        bmp = NULL;
         break;
 
+      case 2:
+      case 3:
+      case 4:
       default:
         free(pic->yuv);
+        pic->yuv = NULL;
         free(pic);
+        pic = NULL;
         printf("wrong pic id\n");
         return;
     }
     if (camera_callback_) {
-      // Add 2 seconds to correct the timestamp, Reason? No reason.
-      // liuxingwei@hesaitech.com
-      camera_callback_(cvMatPic, timestamp + 2 + tz_second_, pic->header.pic_id,
+      camera_callback_(cvMatPic, timestamp + tz_second_, pic->header.pic_id,
                        need_remap_);
     }
     free(pic->yuv);
@@ -178,8 +194,8 @@ void PandoraCamera::processPic() {
   }
 }
 
-bool PandoraCamera::loadIntrinsics(const std::vector<cv::Mat> &cameras_k,
-                                   const std::vector<cv::Mat> &cameras_d) {
+bool PandoraCamera::loadIntrinsics(const std::vector<cv::Mat> cameras_k,
+                                   const std::vector<cv::Mat> cameras_d) {
   for (int i = 0; i < CAMERA_NUM; i++) {
     cv::Mat mapx = cv::Mat(HesaiLidarSDK_IMAGE_SIZE, CV_32FC1);
     cv::Mat mapy = cv::Mat(HesaiLidarSDK_IMAGE_SIZE, CV_32FC1);
diff --git a/pandora/pandora_driver/src/pandora_camera.h b/pandora/pandora_driver/src/pandora_camera.h
index 059ce72..b39393d 100644
--- a/pandora/pandora_driver/src/pandora_camera.h
+++ b/pandora/pandora_driver/src/pandora_camera.h
@@ -17,8 +17,6 @@
 #ifndef SRC_PANDORA_CAMERA_H_
 #define SRC_PANDORA_CAMERA_H_
 
-#include <boost/function.hpp>
-#include <opencv2/opencv.hpp>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 #include <pthread.h>
@@ -28,6 +26,9 @@
 #include <string>
 #include <vector>
 
+#include <boost/function.hpp>
+#include <opencv2/opencv.hpp>
+
 #include "src/pandora_client.h"
 
 namespace apollo {
@@ -50,7 +51,7 @@ class PandoraCamera {
    *        type       				The device type
    */
   PandoraCamera(
-      const std::string &device_ip, const uint16_t pandoraCameraPort,
+      std::string device_ip, const uint16_t pandoraCameraPort,
       boost::function<void(boost::shared_ptr<cv::Mat> matp, double timestamp,
                            int picid, bool distortion)>
           camera_callback,
@@ -62,15 +63,15 @@ class PandoraCamera {
    * @param angle The start angle
    */
 
-  bool loadIntrinsics(const std::vector<cv::Mat> &cameras_k,
-                      const std::vector<cv::Mat> &cameras_d);
+  bool loadIntrinsics(const std::vector<cv::Mat> cameras_k,
+                      const std::vector<cv::Mat> cameras_d);
 
   int Start();
   void Stop();
   void pushPicture(PandoraPic *pic);
 
  private:
-  void processPic();
+  void processPic(int pic_id);
 
   int decompressJpeg(uint8_t *jpgBuffer, const uint32_t jpgSize, uint8_t **bmp,
                      uint32_t *bmpSize);
@@ -81,15 +82,15 @@ class PandoraCamera {
   void yuvToRgb(const int iY, const int iU, const int iV, int *iR, int *iG,
                 int *iB);
 
-  pthread_mutex_t pic_lock_;
-  sem_t pic_sem_;
-  boost::thread *process_pic_thread_;
+  pthread_mutex_t pic_lock_[CAMERA_NUM];
+  sem_t pic_sem_[CAMERA_NUM];
+  boost::thread *process_pic_thread_[CAMERA_NUM];
   bool continue_process_pic_;
   bool need_remap_;
   std::string ip_;
   uint16_t camera_port_;
   void *pandora_client_;
-  std::list<PandoraPic *> pic_list_;
+  std::list<PandoraPic *> pic_list_[CAMERA_NUM];
   std::vector<cv::Mat> mapx_;
   std::vector<cv::Mat> mapy_;
   boost::function<void(boost::shared_ptr<cv::Mat> matp, double timestamp,
diff --git a/pandora/pandora_driver/src/pandora_client.c b/pandora/pandora_driver/src/pandora_client.c
index f8badd6..9151652 100644
--- a/pandora/pandora_driver/src/pandora_client.c
+++ b/pandora/pandora_driver/src/pandora_client.c
@@ -171,17 +171,26 @@ void PandoraClientHeartBeatTask(void* handle) {
     return;
   }
   int ret = 0;
+  
 
   while (!client->exit) {
-    if (-1 == client->cliSocket) {
+    int clifd = -1;
+
+    pthread_mutex_lock(&client->cliSocketLock);
+    if(client->cliSocket != -1) {
+      clifd = client->cliSocket;
+    } else {
+      pthread_mutex_unlock(&client->cliSocketLock);
       sleep(1);
       continue;
     }
-    ret = select_fd(client->cliSocket, 1, WAIT_FOR_WRITE);
+    pthread_mutex_unlock(&client->cliSocketLock);
+
+    ret = select_fd(clifd, 1, WAIT_FOR_WRITE);
     if (ret > 0) {
-      ret = write(client->cliSocket, "HEARTBEAT", strlen("HEARTBEAT"));
+      ret = write(clifd, "HEARTBEAT", strlen("HEARTBEAT"));
       if (ret < 0) {
-        printf("Write Error\n");
+        // printf("Write Error\n");
       }
     }
 
@@ -296,7 +305,7 @@ void PandoraClientTask(void* handle) {
         continue;
       }
 
-      // check the desired frame position packet.
+      // check the desired frame postion packet.
       if (pic->header.position != client->position[pic->header.pic_id]) {
         client->position[pic->header.pic_id] = 0;
         free(pic->yuv);
diff --git a/pandora/pandora_driver/src/tcp_command_client.c b/pandora/pandora_driver/src/tcp_command_client.c
index 13b69c8..22efef0 100644
--- a/pandora/pandora_driver/src/tcp_command_client.c
+++ b/pandora/pandora_driver/src/tcp_command_client.c
@@ -14,12 +14,9 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include "src/tcp_command_client.h"
 #include <arpa/inet.h>
 #include <errno.h>
 #include <fcntl.h>
-#include <linux/sockios.h>
-#include <net/if.h>
 #include <netinet/in.h>
 #include <pthread.h>
 #include <setjmp.h>
@@ -28,14 +25,17 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <sys/ioctl.h>
 #include <sys/ipc.h>
 #include <sys/msg.h>
 #include <sys/socket.h>
 #include <sys/types.h>
 #include <syslog.h>
 #include <unistd.h>
+#include <linux/sockios.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
 #include "src/util.h"
+#include "src/tcp_command_client.h"
 
 typedef struct TcpCommandClient_s {
   pthread_mutex_t lock;
@@ -136,7 +136,7 @@ static int TcpCommand_buildHeader(char* buffer, TC_Command* cmd) {
 
 static PTC_ErrCode tcpCommandClient_SendCmd(TcpCommandClient* client,
                                             TC_Command* cmd) {
-  if (!client || !cmd) {
+  if (!client && !cmd) {
     printf("Bad Parameter\n");
     return PTC_ERROR_BAD_PARAMETER;
   }
diff --git a/pandora/pandora_pointcloud/include/pandora_pointcloud/compensator.h b/pandora/pandora_pointcloud/include/pandora_pointcloud/compensator.h
index 097520b..f997dae 100644
--- a/pandora/pandora_pointcloud/include/pandora_pointcloud/compensator.h
+++ b/pandora/pandora_pointcloud/include/pandora_pointcloud/compensator.h
@@ -24,7 +24,6 @@
 #include <std_msgs/String.h>
 #include <tf2_ros/transform_listener.h>
 #include <Eigen/Eigen>
-#include <string>
 
 namespace apollo {
 namespace drivers {
@@ -32,8 +31,8 @@ namespace pandora {
 
 class Compensator {
  public:
-  Compensator(ros::NodeHandle node, ros::NodeHandle private_nh);
-  virtual ~Compensator() {}
+  Compensator(ros::NodeHandle& node, ros::NodeHandle& private_nh);
+  virtual ~Compensator() = default;
 
  private:
   /**
@@ -46,9 +45,9 @@ class Compensator {
   *   novatel-preprocess broadcast the tf2 transfrom.
   */
   bool query_pose_affine_from_tf2(const double timestamp,
-                                  Eigen::Affine3d* pose);
+                                  Eigen::Affine3d& pose);
   /**
-  * @brief check if message is valid, check width, height, timesatmp.
+  * @brief check if message is valid, check width, height, timestamp.
   *   set timestamp_offset and point data type
   */
   bool check_message(sensor_msgs::PointCloud2ConstPtr msg);
@@ -56,7 +55,7 @@ class Compensator {
   * @brief motion compensation for point cloud
   */
   template <typename Scalar>
-  void motion_compensation(sensor_msgs::PointCloud2::Ptr msg,
+  void motion_compensation(sensor_msgs::PointCloud2::Ptr& msg,
                            const double timestamp_min,
                            const double timestamp_max,
                            const Eigen::Affine3d& pose_min_time,
@@ -65,14 +64,14 @@ class Compensator {
   * @brief get min timestamp and max timestamp from points in pointcloud2
   */
   inline void get_timestamp_interval(
-      sensor_msgs::PointCloud2ConstPtr msg, double* timestamp_min,
-      double* timestamp_max);
+      sensor_msgs::PointCloud2ConstPtr msg, double& timestamp_min,
+      double& timestamp_max);
   /**
   * @brief get point field size by sensor_msgs::datatype
   */
   inline uint get_field_size(const int data_type);
 
-  // subsrcibe pandora pointcloud2 msg.
+  // subscribe pandora pointcloud2 msg.
   ros::Subscriber pointcloud_sub_;
   // publish point cloud2 after motion compensation
   ros::Publisher compensation_pub_;
diff --git a/pandora/pandora_pointcloud/src/compensator.cc b/pandora/pandora_pointcloud/src/compensator.cc
index 0b75810..4c802a2 100644
--- a/pandora/pandora_pointcloud/src/compensator.cc
+++ b/pandora/pandora_pointcloud/src/compensator.cc
@@ -14,8 +14,6 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include <limits>
-#include <string>
 #include "pandora_pointcloud/compensator.h"
 #include "ros/this_node.h"
 
@@ -23,7 +21,7 @@ namespace apollo {
 namespace drivers {
 namespace pandora {
 
-Compensator::Compensator(ros::NodeHandle node, ros::NodeHandle private_nh)
+Compensator::Compensator(ros::NodeHandle& node, ros::NodeHandle& private_nh)
     : tf2_transform_listener_(tf2_buffer_, node),
       x_offset_(-1),
       y_offset_(-1),
@@ -38,15 +36,14 @@ Compensator::Compensator(ros::NodeHandle node, ros::NodeHandle private_nh)
   private_nh.param("topic_pointcloud", topic_pointcloud_,
                    std::string("/apollo/sensor/pandora/hesai40/PointCloud2"));
   private_nh.param("queue_size", queue_size_, 10);
-  private_nh.param("tf_query_timeout", tf_timeout_, 0.1f);
+  private_nh.param("tf_query_timeout", tf_timeout_, float(0.1));
 
   // advertise output point cloud (before subscribing to input data)
   compensation_pub_ = node.advertise<sensor_msgs::PointCloud2>(
       topic_compensated_pointcloud_, queue_size_);
   pointcloud_sub_ =
       node.subscribe(topic_pointcloud_, queue_size_,
-                     &Compensator::pointcloud_callback,
-                     reinterpret_cast<Compensator*>(this));
+                     &Compensator::pointcloud_callback, (Compensator*)this);
 }
 
 void Compensator::pointcloud_callback(
@@ -59,13 +56,13 @@ void Compensator::pointcloud_callback(
   Eigen::Affine3d pose_min_time;
   Eigen::Affine3d pose_max_time;
 
-  double timestamp_min = 0;
-  double timestamp_max = 0;
-  get_timestamp_interval(msg, &timestamp_min, &timestamp_max);
+  double timestamp_min = 0.0;
+  double timestamp_max = 0.0;
+  get_timestamp_interval(msg, timestamp_min, timestamp_max);
 
   // compensate point cloud, remove nan point
-  if (query_pose_affine_from_tf2(timestamp_min, &pose_min_time) &&
-      query_pose_affine_from_tf2(timestamp_max, &pose_max_time)) {
+  if (query_pose_affine_from_tf2(timestamp_min, pose_min_time) &&
+      query_pose_affine_from_tf2(timestamp_max, pose_max_time)) {
     // we change message after motion compensation
     sensor_msgs::PointCloud2::Ptr q_msg(new sensor_msgs::PointCloud2());
     *q_msg = *msg;
@@ -77,10 +74,10 @@ void Compensator::pointcloud_callback(
 }
 
 inline void Compensator::get_timestamp_interval(
-    sensor_msgs::PointCloud2ConstPtr msg, double* timestamp_min,
-    double* timestamp_max) {
-  *timestamp_max = 0.0;
-  *timestamp_min = std::numeric_limits<double>::max();
+    sensor_msgs::PointCloud2ConstPtr msg, double& timestamp_min,
+    double& timestamp_max) {
+  timestamp_max = 0.0;
+  timestamp_min = std::numeric_limits<double>::max();
   int total = msg->width * msg->height;
 
   // get min time and max time
@@ -89,16 +86,16 @@ inline void Compensator::get_timestamp_interval(
     memcpy(&timestamp, &msg->data[i * msg->point_step + timestamp_offset_],
            timestamp_data_size_);
 
-    if (timestamp < *timestamp_min) {
-      *timestamp_min = timestamp;
+    if (timestamp < timestamp_min) {
+      timestamp_min = timestamp;
     }
-    if (timestamp > *timestamp_max) {
-      *timestamp_max = timestamp;
+    if (timestamp > timestamp_max) {
+      timestamp_max = timestamp;
     }
   }
 }
 
-// TODO(a): if point type is always float, and timestamp is always double?
+// TODO: if point type is always float, and timestamp is always double?
 inline bool Compensator::check_message(
     sensor_msgs::PointCloud2ConstPtr msg) {
   // check msg width and height
@@ -110,7 +107,7 @@ inline bool Compensator::check_message(
   int y_data_type = 0;
   int z_data_type = 0;
 
-  // TODO(a): will use a new datastruct with interface to get offset,
+  // TODO: will use a new datastruct with interface to get offset,
   // datatype,datasize...
   for (size_t i = 0; i < msg->fields.size(); ++i) {
     const sensor_msgs::PointField& f = msg->fields[i];
@@ -157,7 +154,7 @@ inline bool Compensator::check_message(
 }
 
 bool Compensator::query_pose_affine_from_tf2(const double timestamp,
-                                             Eigen::Affine3d* pose) {
+                                             Eigen::Affine3d& pose) {
   ros::Time query_time(timestamp);
   std::string err_string;
   if (!tf2_buffer_.canTransform("world", child_frame_id_, query_time,
@@ -178,7 +175,7 @@ bool Compensator::query_pose_affine_from_tf2(const double timestamp,
     return false;
   }
 
-  tf::transformMsgToEigen(stamped_transform.transform, *pose);
+  tf::transformMsgToEigen(stamped_transform.transform, pose);
   // ROS_DEBUG_STREAM("pose matrix : " << pose);
   return true;
 }
@@ -211,7 +208,7 @@ inline uint Compensator::get_field_size(const int datatype) {
 }
 
 template <typename Scalar>
-void Compensator::motion_compensation(sensor_msgs::PointCloud2::Ptr msg,
+void Compensator::motion_compensation(sensor_msgs::PointCloud2::Ptr& msg,
                                       const double timestamp_min,
                                       const double timestamp_max,
                                       const Eigen::Affine3d& pose_min_time,
@@ -237,72 +234,48 @@ void Compensator::motion_compensation(sensor_msgs::PointCloud2::Ptr msg,
 
   // Threshold for a "significant" rotation from min_time to max_time:
   // The LiDAR range accuracy is ~2 cm. Over 70 meters range, it means an angle
-  // of 0.02 / 70 =
-  // 0.0003 rad. So, we consider a rotation "significant" only if the scalar
-  // part of quaternion is
-  // less than cos(0.0003 / 2) = 1 - 1e-8.
-  if (abs_d < 1.0 - 1.0e-8) {
-    double theta = acos(abs_d);
-    double sin_theta = sin(theta);
-    double c1_sign = (d > 0) ? 1 : -1;
-    for (int i = 0; i < total; ++i) {
-      size_t offset = i * msg->point_step;
-      Scalar* x_scalar =
-          reinterpret_cast<Scalar*>(&msg->data[offset + x_offset_]);
-      if (std::isnan(*x_scalar)) {
-        ROS_DEBUG_STREAM("nan point do not need motion compensation");
-        continue;
-      }
-      Scalar* y_scalar =
-          reinterpret_cast<Scalar*>(&msg->data[offset + y_offset_]);
-      Scalar* z_scalar =
-          reinterpret_cast<Scalar*>(&msg->data[offset + z_offset_]);
-      Eigen::Vector3d p(*x_scalar, *y_scalar, *z_scalar);
-
-      double tp = 0.0;
-      memcpy(&tp, &msg->data[i * msg->point_step + timestamp_offset_],
-             timestamp_data_size_);
-      double t = (timestamp_max - tp) * f;
-
-      Eigen::Translation3d ti(t * translation);
-
-      double c0 = sin((1 - t) * theta) / sin_theta;
-      double c1 = sin(t * theta) / sin_theta * c1_sign;
-      Eigen::Quaterniond qi(c0 * q0.coeffs() + c1 * q1.coeffs());
-
-      Eigen::Affine3d trans = ti * qi;
-      p = trans * p;
-      *x_scalar = p.x();
-      *y_scalar = p.y();
-      *z_scalar = p.z();
-    }
-    return;
-  }
-  // Not a "significant" rotation. Do translation only.
+  // of 0.02 / 70 = 0.0003 rad. So, we consider a rotation "significant" only if
+  // the scalar part of quaternion is less than cos(0.0003 / 2) = 1 - 1e-8.
+  const double theta = acos(abs_d);
+  const double sin_theta = sin(theta);
+  const double c1_sign = (d > 0) ? 1 : -1;
   for (int i = 0; i < total; ++i) {
+    size_t offset = i * msg->point_step;
     Scalar* x_scalar =
-        reinterpret_cast<Scalar*>(&msg->data[i * msg->point_step + x_offset_]);
+        reinterpret_cast<Scalar*>(&msg->data[offset + x_offset_]);
     if (std::isnan(*x_scalar)) {
       ROS_DEBUG_STREAM("nan point do not need motion compensation");
       continue;
     }
     Scalar* y_scalar =
-        reinterpret_cast<Scalar*>(&msg->data[i * msg->point_step + y_offset_]);
+        reinterpret_cast<Scalar*>(&msg->data[offset + y_offset_]);
     Scalar* z_scalar =
-        reinterpret_cast<Scalar*>(&msg->data[i * msg->point_step + z_offset_]);
+        reinterpret_cast<Scalar*>(&msg->data[offset + z_offset_]);
     Eigen::Vector3d p(*x_scalar, *y_scalar, *z_scalar);
 
     double tp = 0.0;
     memcpy(&tp, &msg->data[i * msg->point_step + timestamp_offset_],
            timestamp_data_size_);
     double t = (timestamp_max - tp) * f;
+
     Eigen::Translation3d ti(t * translation);
 
-    p = ti * p;
+    if (abs_d < 1.0 - 1.0e-8) {
+      // "significant". Do both rotation and translation.
+      double c0 = sin((1 - t) * theta) / sin_theta;
+      double c1 = sin(t * theta) / sin_theta * c1_sign;
+      Eigen::Quaterniond qi(c0 * q0.coeffs() + c1 * q1.coeffs());
+      Eigen::Affine3d trans = ti * qi;
+      p = trans * p;
+    } else {
+      // Not a "significant" rotation. Do translation only.
+      p = ti * p;
+    }
     *x_scalar = p.x();
     *y_scalar = p.y();
     *z_scalar = p.z();
   }
+  return;
 }
 
 }  // namespace pandora