init

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8)
+
+project(mapping)
+
+set(CMAKE_BUILD_TYPE "RelWithDebInfo")
+#set(CMAKE_BUILD_TYPE "Release")
+#set(CMAKE_BUILD_TYPE "Debug")
+message(STATUS "CMAKE_BUILD_TYPE: " ${CMAKE_BUILD_TYPE})
+
+set(CMAKE_CXX_STANDARD 14)
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
+
+include_directories(/opt/ros/melodic/include)
+link_directories(/opt/ros/melodic/lib)
+
+find_package(PCL REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system filesystem regex program_options thread)
+find_package(Glog REQUIRED)
+find_package(GFlags REQUIRED)
+find_package(PCAP REQUIRED)
+
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS}
+    /opt/ros/melodic/lib)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/libs)
+add_subdirectory(apps)
+add_subdirectory(libs)
+
+
+
+

apps/CMakeLists.txt

+find_package(PCL REQUIRED)
+
+add_executable(mapping mapping.cpp)
+
+target_include_directories(mapping
+    PUBLIC
+    ${PCL_INCLUDE_DIRS})
+
+
+target_link_libraries(mapping
+    ${PCL_LIBRARIES}
+    register
+    lidar
+    ${Boost_LIBRARIES}
+    task_server
+    ${GLOG_LIBRARIES}
+    ${GFLAGS_LIBRARIES})
+#/opt/ros/melodic/lib/libclass_loader.so

apps/cmake/catkinConfig.cmake

+# generated from catkin/cmake/catkinConfig.cmake.in
+# which overlays the default template catkin/cmake/template/pkgConfig.cmake.in
+#
+# :outvar catkin_INCLUDE_DIRS: contains the include dirs of all searched components.
+#    For use with CMake ``include_directories(${catkin_INCLUDE_DIRS})``.
+# :outvar catkin_LIBRARY_DIRS: contains the library dirs of all searched components.
+#    For use with CMake ``link_directories(${catkin_LIBRARY_DIRS})``.
+# :outvar catkin_LIBRARIES: contains the libraries of all searched components.
+#    For use with CMake ``target_link_libraries(<target> ${catkin_LIBRARIES})``.
+# :outvar <comp>_INCLUDE_DIRS/_LIBRARY_DIRS/_LIBRARIES:
+#    contains the include dirs / library dirs / libraries of the searched component <comp>.
+
+if(CATKIN_TOPLEVEL_FIND_PACKAGE OR NOT CATKIN_TOPLEVEL)
+  set(catkin_EXTRAS_DIR "${catkin_DIR}")
+
+  # prevent multiple inclusion from repeated find_package() calls in non-workspace context
+  # as long as this variable is in the scope the variables from all.cmake are also, so no need to be evaluated again
+  if(NOT DEFINED _CATKIN_CONFIG_ALL_INCLUDED_)
+    set(_CATKIN_CONFIG_ALL_INCLUDED_ TRUE)
+    include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
+  endif()
+endif()
+
+# skip setting find_package() variables when discovered from toplevel.cmake
+if(CATKIN_TOPLEVEL_FIND_PACKAGE)
+  return()
+endif()
+
+
+# prevent usage with wrong case
+if(CATKIN_FIND_COMPONENTS OR CATKIN_FIND_REQUIRED OR CATKIN_FIND_QUIETLY)
+  message(FATAL_ERROR "find_package() only supports lower-case package name 'catkin'")
+endif()
+
+# mark as found
+if(NOT catkin_FOUND)
+  set(catkin_FOUND)
+  set(CATKIN_PACKAGE_PREFIX "" CACHE STRING "Prefix to apply to package generated via gendebian")
+endif()
+
+# flag project as catkin-based to distinguish if a find_package()-ed project is a catkin project
+set(catkin_FOUND_CATKIN_PROJECT TRUE)
+
+# XXXX don't overwrite catkin_* variables when being called recursively
+if(NOT _CATKIN_FIND_ OR _CATKIN_FIND_ EQUAL 0)
+  set(_CATKIN_FIND_ 0)
+  if(catkin_FIND_COMPONENTS)
+    set(catkin_INCLUDE_DIRS "")
+    set(catkin_LIBRARIES "")
+    set(catkin_LIBRARY_DIRS "")
+    set(catkin_EXPORTED_TARGETS "")
+  endif()
+endif()
+
+# increment recursion counter
+math(EXPR _CATKIN_FIND_ "${_CATKIN_FIND_} + 1")
+
+# find all components
+if(catkin_FIND_COMPONENTS)
+  foreach(component ${catkin_FIND_COMPONENTS})
+    string(TOLOWER "${component}" component_lower)
+    # skip catkin since it does not make sense as a component
+    if(NOT ${component_lower} STREQUAL "catkin")
+
+      # get search paths from CMAKE_PREFIX_PATH (which includes devel space)
+      set(paths "")
+      foreach(path ${CMAKE_PREFIX_PATH})
+        if(IS_DIRECTORY ${path}/share/${component}/cmake)
+          list(APPEND paths ${path}/share/${component}/cmake)
+        endif()
+      endforeach()
+
+      # find package component
+      if(catkin_FIND_REQUIRED)
+        # try without REQUIRED first
+        find_package(${component} NO_MODULE PATHS ${paths}
+          NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+        if(NOT ${component}_FOUND)
+          # show better message to help users with the CMake error message coming up
+          message(STATUS "Could not find the required component '${component}'. "
+            "The following CMake error indicates that you either need to install the package "
+            "with the same name or change your environment so that it can be found.")
+          find_package(${component} REQUIRED NO_MODULE PATHS ${paths}
+            NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+        endif()
+      elseif(catkin_FIND_QUIETLY)
+        find_package(${component} QUIET NO_MODULE PATHS ${paths}
+          NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+      else()
+        find_package(${component} NO_MODULE PATHS ${paths}
+          NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+      endif()
+
+      # append component-specific variables to catkin_* variables
+      list_append_unique(catkin_INCLUDE_DIRS ${${component}_INCLUDE_DIRS})
+
+      # merge build configuration keywords with library names to correctly deduplicate
+      catkin_pack_libraries_with_build_configuration(catkin_LIBRARIES ${catkin_LIBRARIES})
+      catkin_pack_libraries_with_build_configuration(_libraries ${${component}_LIBRARIES})
+      list_append_deduplicate(catkin_LIBRARIES ${_libraries})
+      # undo build configuration keyword merging after deduplication
+      catkin_unpack_libraries_with_build_configuration(catkin_LIBRARIES ${catkin_LIBRARIES})
+
+      list_append_unique(catkin_LIBRARY_DIRS ${${component}_LIBRARY_DIRS})
+      list(APPEND catkin_EXPORTED_TARGETS ${${component}_EXPORTED_TARGETS})
+    endif()
+  endforeach()
+  list_insert_in_workspace_order(catkin_INCLUDE_DIRS ${catkin_INCLUDE_DIRS})
+  list_insert_in_workspace_order(catkin_LIBRARY_DIRS ${catkin_LIBRARY_DIRS})
+endif()
+
+# add dummy target to catkin_EXPORTED_TARGETS if empty
+if(NOT catkin_EXPORTED_TARGETS)
+  if(NOT TARGET _catkin_empty_exported_target)
+    add_custom_target(_catkin_empty_exported_target)
+  endif()
+  list(APPEND catkin_EXPORTED_TARGETS _catkin_empty_exported_target)
+endif()
+
+# decrement recursion counter
+math(EXPR _CATKIN_FIND_ "${_CATKIN_FIND_} - 1")
+
+if(_CATKIN_FIND_ EQUAL 0)
+  # store found components (from the fist level only) for validation in catkin_package() that they are build dependencies
+  list(APPEND catkin_ALL_FOUND_COMPONENTS ${catkin_FIND_COMPONENTS})
+endif()

apps/mapping (copy).cpp

+#include "utils/basic_types.h"
+
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include "pcl/point_types.h"
+#include "pcl/point_cloud.h"
+
+using namespace std;
+int main(int argc, char *argv[])
+{
+
+}
+
+//cout << "000" << endl;
+//rosbag::Bag bag("/home/juefx/wdw/label/slam/cartographer.bag");
+//cout << "111" << endl;
+//rosbag::View view(bag);
+//cout << "222" << endl;
+//pcl::PointCloud<pcl::PointXYZ> temp_pc;
+//for(const rosbag::MessageInstance &m :view)
+//{
+//    sensor_msgs::PointCloud2::ConstPtr s_point = m.instantiate<sensor_msgs::PointCloud2>();
+
+//   if(s_point != nullptr)
+//   {
+//       sensor_msgs::PointCloud2 s_m_point = *s_point;
+//       Eigen::Matrix4d transform;
+//       pcl::PointCloud<pcl::PointXYZ> cloud;
+////           for(auto field : s_m_point.fields){
+////               cout << field.name << endl;
+////           }
+//       cout << 1 << endl;
+//       pcl::fromROSMsg(s_m_point, cloud);
+//   }
+//}

apps/mapping.cpp

+#define PCL_NO_PRECOMPILE
+#include <gflags/gflags.h>
+#include "functions.h"
+#include "register/gicp.h"
+#include "task_server/task_server.h"
+
+DEFINE_string(base_dir, "", "base_dir");
+DEFINE_string(mesh, "", "mesh");
+DEFINE_string(overwrite, "on", "overwrite");
+DEFINE_string(maplidar, "back", "maplidar");
+DEFINE_string(filter, "on", "filter");
+DEFINE_string(period, "", "period");
+DEFINE_string(pc_mode, "pcap", "pc_mode");
+DEFINE_string(traj_mode, "ppknode", "traj_mode");
+DEFINE_string(downsample, "", "downsample");
+DEFINE_string(bag_mode, "disorted", "bag_mode");
+DEFINE_string(option, "", "option");
+DEFINE_string(task_mode, "", "single");
+DEFINE_string(pcapfile, "", "pcapfile");
+DEFINE_string(func, "", "func");
+
+void signaleHandle(const char* data, int size) {
+  string str = data;
+  LOG(INFO) << "signaleHandle: " << str;
+}
+
+int main(int argc, char* argv[]) {
+  FLAGS_log_dir = "/home/juefx/log";
+  system("ulimit -c 10000000");
+  LOG(INFO) << "argc : " << argc;
+  for (int i = 0; i < argc; i++) {
+    LOG(INFO) << "argv[" << i << "] : " << string(argv[i]);
+  }
+  gflags::ParseCommandLineFlags(&argc, &argv, true);
+  google::InitGoogleLogging(argv[0]);
+  google::InstallFailureSignalHandler();
+  google::InstallFailureWriter(&signaleHandle);
+  FLAGS_alsologtostderr = true;
+  FLAGS_colorlogtostderr = true;
+  FLAGS_stderrthreshold = 0;
+  FLAGS_logbufsecs = 0;
+
+  setenv("TZ", "GMT-0", 1);
+  tzset();
+
+  std::string exePath =
+      boost::filesystem::initial_path<boost::filesystem::path>().string();
+  system(string("rm " + exePath + "/core").c_str());
+  boost::shared_ptr<StrategyControl> stageControl =
+      boost::shared_ptr<StrategyControl>(
+          new StrategyControl(FLAGS_overwrite, FLAGS_maplidar, FLAGS_filter,
+                              FLAGS_pc_mode, FLAGS_traj_mode, FLAGS_period,
+                              FLAGS_option, FLAGS_bag_mode, FLAGS_task_mode));
+  if (stageControl->getOption() == "pcaptime") {
+    ParamsServer paramsServer(FLAGS_base_dir);
+    paramsServer.findPcapsTimeRange();
+    return 1;
+  }
+  if (stageControl->getOption().find("rawframe") != string::npos) {
+    //        setenv("TZ", "GMT-0", 1);
+    //        tzset();
+  }
+  vector<string> specified_mesh_vec;
+  if (FLAGS_mesh != "") {
+    boost::split(specified_mesh_vec, FLAGS_mesh, boost::is_any_of(","),
+                 boost::token_compress_on);
+  }
+  boost::shared_ptr<TaskServer> taskServer(
+      new TaskServer(stageControl, specified_mesh_vec));
+  if (FLAGS_base_dir != "") {
+    if ("gather" == FLAGS_func) {
+      Gather(FLAGS_base_dir);
+      return 1;
+    }
+    boost::shared_ptr<ParamsServer> paramsServer =
+        boost::shared_ptr<ParamsServer>(new ParamsServer(FLAGS_base_dir));
+    if (MERGE == stageControl->getTaskMode()) {
+      paramsServer->configRelativeTask();
+    }
+    paramsServer->initTrajBackend();
+    Pointcloud_Source_Mode pcMode = stageControl->getPCMode();
+    if (pcMode == BAG_MODE) {
+      string mesh_dir = FLAGS_base_dir + "/mesh/";
+      bool isMesh = false;
+      if (exists(mesh_dir)) {
+        directory_iterator mesh_iter(mesh_dir);
+        if (is_directory(mesh_iter->path())) {
+          string time_path = mesh_iter->path().string() + "/trace/gps.dat.time";
+          if (is_regular_file(time_path)) {
+            isMesh = true;
+          }
+        }
+      }
+      if (isMesh) {
+        paramsServer->configMeshTaskDescribe();
+        if (stageControl->IsOverWrite() && 0 == specified_mesh_vec.size()) {
+          system(string("rm " + mesh_dir + "/*/*/*.pcd").c_str());
+          system(string("rm " + mesh_dir + "/*/*/*.track").c_str());
+        }
+      } else {
+        paramsServer->configBagTaskDescribe();
+      }
+    } else if (pcMode == PCAP_MODE || pcMode == PCAP_BAG_MODE ||
+               pcMode == PCD_MODE) {
+      string option = stageControl->getOption();
+      if (option.find("rawframe") != string::npos &&
+          FLAGS_pcapfile.find("pcl0") != string::npos) {
+        stageControl.reset(new StrategyControl(
+            FLAGS_overwrite, "top", FLAGS_filter, FLAGS_pc_mode,
+            FLAGS_traj_mode, FLAGS_period, FLAGS_option, FLAGS_bag_mode));
+        taskServer.reset(new TaskServer(stageControl, specified_mesh_vec));
+        paramsServer->configPcapTaskDescribe(FLAGS_pcapfile);
+      } else {
+        paramsServer->configMeshTaskDescribe();
+      }
+    }
+    taskServer->appendParamsServer(paramsServer);
+  } else if (FLAGS_period != "") {
+    map<string, vector<uint32_t>> task_submap_vec =
+        stageControl->getTaskSubmapVec();
+    for (auto iter = task_submap_vec.begin(); iter != task_submap_vec.end();
+         iter++) {
+      string taskPath = iter->first;
+      vector<uint32_t> submapId_vec = iter->second;
+      boost::shared_ptr<ParamsServer> paramsServer =
+          boost::shared_ptr<ParamsServer>(new ParamsServer(taskPath));
+      paramsServer->initTrajBackend();
+
+      paramsServer->configPeriodTaskDescribe(submapId_vec);
+      taskServer->appendParamsServer(paramsServer);
+    }
+  } else if (FLAGS_downsample != "") {
+    downSamplePcd<PointExport>(FLAGS_downsample + "/");
+  }
+  taskServer->processLoop();
+
+  LOG(INFO) << "mapping done!";
+  return 1;
+}

cmake/FindGFlags.cmake

+# - Try to find GFLAGS
+#
+# The following variables are optionally searched for defaults
+#  GFLAGS_ROOT_DIR:            Base directory where all GFLAGS components are found
+#
+# The following are set after configuration is done:
+#  GFLAGS_FOUND
+#  GFLAGS_INCLUDE_DIRS
+#  GFLAGS_LIBRARIES
+#  GFLAGS_LIBRARYRARY_DIRS
+
+include(FindPackageHandleStandardArgs)
+
+set(GFLAGS_ROOT_DIR "" CACHE PATH "Folder contains Gflags")
+
+# We are testing only a couple of files in the include directories
+if(WIN32)
+    find_path(GFLAGS_INCLUDE_DIR gflags/gflags.h
+        PATHS ${GFLAGS_ROOT_DIR}/src/windows)
+else()
+    find_path(GFLAGS_INCLUDE_DIR gflags/gflags.h
+        PATHS ${GFLAGS_ROOT_DIR})
+endif()
+
+if(MSVC)
+    find_library(GFLAGS_LIBRARY_RELEASE
+        NAMES libgflags
+        PATHS ${GFLAGS_ROOT_DIR}
+        PATH_SUFFIXES Release)
+
+    find_library(GFLAGS_LIBRARY_DEBUG
+        NAMES libgflags-debug
+        PATHS ${GFLAGS_ROOT_DIR}
+        PATH_SUFFIXES Debug)
+
+    set(GFLAGS_LIBRARY optimized ${GFLAGS_LIBRARY_RELEASE} debug ${GFLAGS_LIBRARY_DEBUG})
+else()
+    find_library(GFLAGS_LIBRARY gflags)
+endif()
+
+find_package_handle_standard_args(GFLAGS DEFAULT_MSG
+    GFLAGS_INCLUDE_DIR GFLAGS_LIBRARY)
+
+
+if(GFLAGS_FOUND)
+    set(GFLAGS_INCLUDE_DIRS ${GFLAGS_INCLUDE_DIR})
+    set(GFLAGS_LIBRARIES ${GFLAGS_LIBRARY})
+endif()

cmake/FindGeographicLib.cmake

+# Look for GeographicLib
+#
+# Set
+#  GeographicLib_FOUND = GEOGRAPHICLIB_FOUND = TRUE
+#  GeographicLib_INCLUDE_DIRS = /usr/local/include
+#  GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so
+#  GeographicLib_LIBRARY_DIRS = /usr/local/lib
+
+find_library (GeographicLib_LIBRARIES Geographic
+  PATHS "${CMAKE_INSTALL_PREFIX}/../GeographicLib/lib")
+
+if (GeographicLib_LIBRARIES)
+  get_filename_component (GeographicLib_LIBRARY_DIRS
+    "${GeographicLib_LIBRARIES}" PATH)
+  get_filename_component (_ROOT_DIR "${GeographicLib_LIBRARY_DIRS}" PATH)
+  set (GeographicLib_INCLUDE_DIRS "${_ROOT_DIR}/include")
+  set (GeographicLib_BINARY_DIRS "${_ROOT_DIR}/bin")
+  if (NOT EXISTS "${GeographicLib_INCLUDE_DIRS}/GeographicLib/Config.h")
+    # On Debian systems the library is in e.g.,
+    #   /usr/lib/x86_64-linux-gnu/libGeographic.so
+    # so try stripping another element off _ROOT_DIR
+    get_filename_component (_ROOT_DIR "${_ROOT_DIR}" PATH)
+    set (GeographicLib_INCLUDE_DIRS "${_ROOT_DIR}/include")
+    set (GeographicLib_BINARY_DIRS "${_ROOT_DIR}/bin")
+    if (NOT EXISTS "${GeographicLib_INCLUDE_DIRS}/GeographicLib/Config.h")
+      unset (GeographicLib_INCLUDE_DIRS)
+      unset (GeographicLib_LIBRARIES)
+      unset (GeographicLib_LIBRARY_DIRS)
+      unset (GeographicLib_BINARY_DIRS)
+    endif ()
+  endif ()
+  unset (_ROOT_DIR)
+endif ()
+
+include (FindPackageHandleStandardArgs)
+find_package_handle_standard_args (GeographicLib DEFAULT_MSG
+  GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES
+  GeographicLib_INCLUDE_DIRS)
+mark_as_advanced (GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES
+  GeographicLib_INCLUDE_DIRS)

cmake/FindGlog.cmake

+# - Try to find Glog
+#
+# The following variables are optionally searched for defaults
+#  GLOG_ROOT_DIR:            Base directory where all GLOG components are found
+#
+# The following are set after configuration is done:
+#  GLOG_FOUND
+#  GLOG_INCLUDE_DIRS
+#  GLOG_LIBRARIES
+#  GLOG_LIBRARYRARY_DIRS
+
+include(FindPackageHandleStandardArgs)
+
+set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog")
+
+if(WIN32)
+    find_path(GLOG_INCLUDE_DIR glog/logging.h
+        PATHS ${GLOG_ROOT_DIR}/src/windows)
+else()
+    find_path(GLOG_INCLUDE_DIR glog/logging.h
+        PATHS ${GLOG_ROOT_DIR})
+endif()
+
+if(MSVC)
+    find_library(GLOG_LIBRARY_RELEASE libglog_static
+        PATHS ${GLOG_ROOT_DIR}
+        PATH_SUFFIXES Release)
+
+    find_library(GLOG_LIBRARY_DEBUG libglog_static
+        PATHS ${GLOG_ROOT_DIR}
+        PATH_SUFFIXES Debug)
+
+    set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG})
+else()
+    find_library(GLOG_LIBRARY glog
+        PATHS ${GLOG_ROOT_DIR}
+        PATH_SUFFIXES
+            lib
+            lib64)
+endif()
+
+find_package_handle_standard_args(GLOG DEFAULT_MSG
+    GLOG_INCLUDE_DIR GLOG_LIBRARY)
+
+if(GLOG_FOUND)
+    set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR})
+    set(GLOG_LIBRARIES ${GLOG_LIBRARY})
+endif()

cmake/FindPCAP.cmake

+# - Try to find libpcap include dirs and libraries
+#
+# Usage of this module as follows:
+#
+#     find_package(PCAP)
+#
+# Variables used by this module, they can change the default behaviour and need
+# to be set before calling find_package:
+#
+#  PCAP_ROOT_DIR             Set this variable to the root installation of
+#                            libpcap if the module has problems finding the
+#                            proper installation path.
+#
+# Variables defined by this module:
+#
+#  PCAP_FOUND                System has libpcap, include and library dirs found
+#  PCAP_INCLUDE_DIR          The libpcap include directories.
+#  PCAP_LIBRARY              The libpcap library (possibly includes a thread
+#                            library e.g. required by pf_ring's libpcap)
+#  HAVE_PF_RING              If a found version of libpcap supports PF_RING
+
+find_path(PCAP_ROOT_DIR
+    NAMES include/pcap.h
+)
+
+find_path(PCAP_INCLUDE_DIR
+    NAMES pcap.h
+    HINTS ${PCAP_ROOT_DIR}/include
+)
+
+find_library(PCAP_LIBRARY
+    NAMES pcap
+    HINTS ${PCAP_ROOT_DIR}/lib
+)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(PCAP DEFAULT_MSG
+    PCAP_LIBRARY
+    PCAP_INCLUDE_DIR
+)
+
+include(CheckCSourceCompiles)
+set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY})
+check_c_source_compiles("int main() { return 0; }" PCAP_LINKS_SOLO)
+set(CMAKE_REQUIRED_LIBRARIES)
+
+# check if linking against libpcap also needs to link against a thread library
+if (NOT PCAP_LINKS_SOLO)
+    find_package(Threads)
+    if (THREADS_FOUND)
+        set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY} ${CMAKE_THREAD_LIBS_INIT})
+        check_c_source_compiles("int main() { return 0; }" PCAP_NEEDS_THREADS)
+        set(CMAKE_REQUIRED_LIBRARIES)
+    endif ()
+    if (THREADS_FOUND AND PCAP_NEEDS_THREADS)
+        set(_tmp ${PCAP_LIBRARY} ${CMAKE_THREAD_LIBS_INIT})
+        list(REMOVE_DUPLICATES _tmp)
+        set(PCAP_LIBRARY ${_tmp}
+            CACHE STRING "Libraries needed to link against libpcap" FORCE)
+    else ()
+        message(FATAL_ERROR "Couldn't determine how to link against libpcap")
+    endif ()
+endif ()
+
+include(CheckFunctionExists)
+set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY})
+check_function_exists(pcap_get_pfring_id HAVE_PF_RING)
+set(CMAKE_REQUIRED_LIBRARIES)
+
+mark_as_advanced(
+    PCAP_ROOT_DIR
+    PCAP_INCLUDE_DIR
+    PCAP_LIBRARY
+)

libs/CMakeLists.txt

+add_subdirectory(io)
+add_subdirectory(trajectory)
+add_subdirectory(lidar)
+add_subdirectory(reader)
+add_subdirectory(task)
+add_subdirectory(utils)
+add_subdirectory(params_control)
+add_subdirectory(transform)
+add_subdirectory(dynamic_remover)
+add_subdirectory(task_server)
+add_subdirectory(segmentor)
+add_subdirectory(register)
+add_subdirectory(yaml-cpp-master)

libs/dynamic_remover/CMakeLists.txt

+add_library(dynamic_remover STATIC dynamic_remover.cpp
+    grid_coord.cpp)
+
+target_link_libraries(dynamic_remover
+    lidar
+    ${Boost_LIBRARIES}
+    ${PCL_LIBRARIES})

libs/dynamic_remover/grid_coord.cpp

+#include "grid_coord.h"
+#include <fstream>
+#include <sstream>
+
+const int ClosestTrajCnt = 3;
+
+GridCoord::GridCoord(float resolution, float length)
+    :resolution_(resolution),
+      length_(length)
+{
+    float half_length = length_ / 2;
+    half_grid_width = half_length / resolution_;
+    half_grid_width++;
+    grid_width_ = half_grid_width * 2;
+
+    grids_ = (Grid***)malloc(grid_width_ * sizeof (Grid**));
+    for(unsigned int i = 0; i < grid_width_; i++) {
+        grids_[i] = (Grid**)malloc(grid_width_ * sizeof (Grid*));
+        for(unsigned int j = 0; j < grid_width_; j++){
+            Vector2i index(i - half_grid_width, j - half_grid_width);
+            grids_[i][j] = new Grid(ClosestTrajCnt, gridCenter(index));
+        }
+    }
+}
+
+GridCoord::~GridCoord()
+{
+    for(unsigned int i = 0; i < grid_width_; i++) {
+        for(unsigned int j = 0; j < grid_width_; j++){
+            delete grids_[i][j];
+        }
+        free(grids_[i]);
+    }
+    free(grids_);
+}
+
+void GridCoord::updateArea(const TrajPoint &trajPoint)
+{
+    float areaRange = 80;
+
+    Vector3d trajTranslation = trajPoint.translation;
+    Eigen::Vector2f curr_traj_point(trajTranslation.x(), trajTranslation.y());
+    Vector2f left_bottom(trajTranslation.x() - areaRange, trajTranslation.y() - areaRange);
+    Vector2f right_top(trajTranslation.x() + areaRange, trajTranslation.y() + areaRange);
+
+    Vector2i left_bottom_index = gridRawIndex(left_bottom);
+    Vector2i right_top_index = gridRawIndex(right_top);
+
+    for(int r = left_bottom_index.x(); r < right_top_index.x(); r++){
+        for(int c = left_bottom_index.y(); c < right_top_index.y(); c++){
+            Vector2i index(r + half_grid_width, c + half_grid_width);
+            if(index.x() >= grid_width_ || index.y() >= grid_width_ ||
+                    index.x() < 0 || index.y() < 0){
+                continue;
+            }
+            Vector2f curr_grid_center = gridCenter(Vector2i(r, c));
+            float dis = (curr_traj_point - curr_grid_center).norm();
+            grids_[index.x()][index.y()]->update(dis, trajPoint);
+        }
+    }
+
+
+}
+
+void GridCoord::peakTrajDrationAround()
+{
+    for(unsigned int i = 0; i < grid_width_; i++) {
+        for(unsigned int j = 0; j < grid_width_; j++){
+            grids_[i][j]->peakTrajDrationAround();
+        }
+    }
+}
+
+int GridCoord::isDynamicallyAccepted(float x, float y, float dis, const TrajPoint &trajPoint)
+{
+    Vector2f curr_point(x, y);
+    Vector2i curr_index = gridIndex(curr_point);
+
+    if(curr_index.x() >= grid_width_ || curr_index.y() >= grid_width_ ||
+            curr_index.x() < 0 || curr_index.y() < 0){
+
+        return -2;
+    }
+    Grid* grid = grids_[curr_index.x()][curr_index.y()];
+    int fit_num = grid->fit(trajPoint);
+
+    if(0 == fit_num){
+        if(dis < 80){
+            return 0;
+        }
+    }else if(1 == fit_num){
+        if(dis < 30){
+            return 1;
+        }
+    }else if(2 == fit_num){
+        if(dis < 20){
+            return 2;
+        }
+    }
+
+    if(dis < 20){
+        return 4;
+    }
+    return -1;
+}
+
+void GridCoord::printGrid()
+{
+    string gridInfoPath = "/tmp/gridInfo/";
+    if(!boost::filesystem::exists(gridInfoPath)){
+        boost::filesystem::create_directories(gridInfoPath);
+    }
+
+    for(int i = 0; i < grid_width_; i++) {
+        for(int j = 0; j < grid_width_; j++){
+            vector<PlatForm> platFormVec = grids_[i][j]->getPlatFormVec();
+            if(platFormVec.size() == 0){
+                continue;
+            }
+            auto PlatForm = platFormVec.front();
+            Vector2f position =  PlatForm.getPosition();
+            auto closestTrajDurationVec = PlatForm.getClosestTrajDurationVec();
+            if(closestTrajDurationVec.size() < 3){
+                continue;
+            }
+            if(abs(i - (int)half_grid_width) > 150 || abs(j - (int)half_grid_width) > 150){
+                continue;
+            }
+            string path = gridInfoPath + to_string(i) + "_" + to_string(j) + ".txt";
+            std::ofstream ofs(path);
+            if(!ofs){
+                LOG(WARNING) << path << " load fail!";
+                return;
+            }
+            ofs << setprecision(15) << position[0] << " " << position[1] << " " << 0 << " " << 0 << endl;
+            for(auto& trajDuration : closestTrajDurationVec){
+                ofs << setprecision(15) << trajDuration.trajPoint_.translation[0] << " " << trajDuration.trajPoint_.translation[1] << " "
+                    << trajDuration.trajPoint_.translation[2] << " " << trajDuration.trajPoint_.nodeId << endl;
+            }
+            ofs.close();
+        }
+    }
+}

libs/io/CMakeLists.txt

+add_library(io STATIC textio.cpp)
+
+
+target_link_libraries(io
+    ${PCAP_LIBRARY})

libs/io/PcapFileReader.h

+#ifndef __PacketFileReader_h
+#define __PacketFileReader_h
+
+#include <pcap.h>
+#include <string>
+#include <iostream>
+
+// Some versions of libpcap do not have PCAP_NETMASK_UNKNOWN
+#if !defined(PCAP_NETMASK_UNKNOWN)
+#define PCAP_NETMASK_UNKNOWN 0xffffffff
+#endif
+
+class PcapFileReader
+{
+public:
+  PcapFileReader() { this->PCAPFile = 0; }
+
+  ~PcapFileReader() { this->Close(); }
+
+  // This function is called to read a savefile .pcap
+  // 1-Open a savefile in the tcpdump/libcap format to read packet
+  // 2-A packet filter is then compile to convert an high level filtering
+  //  expression in a program that can be interpreted by the kernel-level filtering engine
+  // 3- The compiled filter is then associate to the capture
+  bool Open(const std::string& filename)
+  {
+    char errbuff[PCAP_ERRBUF_SIZE];
+    pcap_t* pcapFile = pcap_open_offline(filename.c_str(), errbuff);
+    if (!pcapFile)
+    {
+      this->LastError = errbuff;
+      return false;
+    }
+
+    struct bpf_program filter;
+
+    if (pcap_compile(pcapFile, &filter, "udp", 0, PCAP_NETMASK_UNKNOWN) == -1)
+    {
+      this->LastError = pcap_geterr(pcapFile);
+      return false;
+    }
+
+    if (pcap_setfilter(pcapFile, &filter) == -1)
+    {
+      this->LastError = pcap_geterr(pcapFile);
+      return false;
+    }
+
+    this->FileName = filename;
+    this->PCAPFile = pcapFile;
+    this->StartTime.tv_sec = this->StartTime.tv_usec = 0;
+    return true;
+  }
+
+  bool IsOpen() { return (this->PCAPFile != 0); }
+
+  void Close()
+  {
+    if (this->PCAPFile)
+    {
+      pcap_close(this->PCAPFile);
+      this->PCAPFile = 0;
+      this->FileName.clear();
+    }
+  }
+
+  const std::string& GetLastError() { return this->LastError; }
+
+  const std::string& GetFileName() { return this->FileName; }
+
+  void GetFilePosition(fpos_t* position)
+  {
+#ifdef _MSC_VER
+    pcap_fgetpos(this->PCAPFile, position);
+#else
+    FILE* f = pcap_file(this->PCAPFile);
+    fgetpos(f, position);
+#endif
+  }
+
+  void SetFilePosition(fpos_t* position)
+  {
+#ifdef _MSC_VER
+    pcap_fsetpos(this->PCAPFile, position);
+#else
+    FILE* f = pcap_file(this->PCAPFile);
+    fsetpos(f, position);
+#endif
+  }
+
+  FILE* GetFILE(){
+      return pcap_file(this->PCAPFile);
+  }
+
+  bool NextPacket(const unsigned char*& data, unsigned int& dataLength, double& timeSinceStart,
+    pcap_pkthdr** headerReference = NULL)
+  {
+    if (!this->PCAPFile)
+    {
+      return false;
+    }
+
+    struct pcap_pkthdr* header;
+    int returnValue = pcap_next_ex(this->PCAPFile, &header, &data);
+    if (returnValue < 0)
+    {
+//      this->Close();
+      return false;
+    }
+
+    if (headerReference != NULL)
+    {
+      *headerReference = header;
+      dataLength = header->len;
+      timeSinceStart = GetElapsedTime(header->ts, this->StartTime);
+      return true;
+    }
+
+    // The ethernet header is 42 bytes long; unnecessary
+    const unsigned int bytesToSkip = 42;
+    dataLength = header->len - bytesToSkip;
+    data = data + bytesToSkip;
+    timeSinceStart = GetElapsedTime(header->ts, this->StartTime);
+    return true;
+  }
+
+protected:
+  double GetElapsedTime(const struct timeval& end, const struct timeval& start)
+  {
+    return (end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) / 1000000.00;
+  }
+
+  pcap_t* PCAPFile;
+  std::string FileName;
+  std::string LastError;
+  struct timeval StartTime;
+};
+
+#endif

libs/io/textio.cpp

+#include "textio.h"
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/split.hpp>
+
+TextIO::TextIO(const string &path, const string &range)
+    :path_(path),
+      range_(range)
+{
+
+}
+
+bool TextIO::load()
+{
+    std::ifstream ifs(path_);
+    if(!ifs){
+        LOG(WARNING) << path_ << " load fail!";
+        return false;
+    }
+    if(!parse_cb_){
+        LOG(WARNING) << "boost::function parse_ is null!";
+        return false;
+    }
+    string line;
+    getline(ifs, line);
+    getline(ifs, line);
+    vector<string> line_vec;
+    boost::split(line_vec, line, boost::is_any_of(range_), boost::token_compress_on);
+    parse_cb_(line_vec);
+    vector<string> lines;
+    while(getline(ifs, line)) {
+        lines.push_back(line);
+    }
+    ifs.close();
+    multiThreadParseText(lines);
+    return true;
+}
+
+void TextIO::multiThreadParseText(const vector<string> &lines)
+{
+    vector<pair<uint32_t, uint32_t>> indexPeriod = getIndexPeriod(lines.size());
+    vector<ThreadPtr> thread_vec;
+    for(uint8_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
+
+        thread_vec.emplace_back(ThreadPtr(new boost::thread(boost::bind(&TextIO::parseTextForPeriod,
+                                                                        this,
+                                                                        &lines,
+                                                                        indexPeriod[thread_index]))));
+    }
+    for(uint8_t thread_index = 0; thread_index < thread_vec.size(); thread_index++){
+        thread_vec[thread_index]->join();
+        thread_vec[thread_index].reset();
+    }
+}
+
+void TextIO::parseTextForPeriod(const vector<string> *lines,
+                                pair<uint32_t, uint32_t> duration)
+{
+    uint32_t start_index = duration.first;
+    uint32_t index_cnt = duration.second;
+    uint32_t final_index = start_index + index_cnt;
+    for(uint32_t index = start_index; index < final_index; index++){
+        vector<string> line_vec;
+        boost::split(line_vec, lines->at(index), boost::is_any_of(range_), boost::token_compress_on);
+        parse_cb_(line_vec);
+    }
+}
+
+void TextIO::setParseCallback(boost::function<void (const vector<string> &)> cb)
+{
+    parse_cb_ = cb;
+}
+
+string TextIO::getPath()
+{
+    return path_;
+}

libs/io/textio.h

+#ifndef TEXTIO_H
+#define TEXTIO_H
+#include "utils/basic_functions.h"
+#include <boost/function.hpp>
+
+class TextIO
+{
+public:
+    TextIO(const string &path, const string &range = string());
+
+    bool load();
+
+    void setParseCallback(boost::function<void(const vector<string>&)> cb);
+
+    string getPath();
+private:
+    void multiThreadParseText(const vector<string> &lines);
+
+    void parseTextForPeriod(const vector<string> *lines, pair<uint32_t, uint32_t> duration);
+private:
+    string path_;
+
+    string range_;
+
+    boost::function<void(const vector<string>&)> parse_cb_;
+};
+
+#endif // TEXTIO_H

libs/lidar/CMakeLists.txt

+find_package(PCL REQUIRED)
+
+file(GLOB_RECURSE SRC *.cpp *.cc)
+
+add_library(lidar STATIC
+    ${SRC})
+
+target_include_directories(lidar
+    PUBLIC
+    ${PCL_INCLUDE_DIRS})
+
+
+set(catkin_LIBRARIES /opt/ros/melodic/lib/libpcl_ros_filter.so
+    /opt/ros/melodic/lib/libpcl_ros_tf.so
+    /usr/lib/x86_64-linux-gnu/libpcl_kdtree.so
+    /usr/lib/x86_64-linux-gnu/libpcl_search.so
+    /usr/lib/x86_64-linux-gnu/libpcl_features.so
+    /usr/lib/x86_64-linux-gnu/libpcl_sample_consensus.so
+    /usr/lib/x86_64-linux-gnu/libpcl_filters.so
+    /usr/lib/x86_64-linux-gnu/libpcl_ml.so
+    /usr/lib/x86_64-linux-gnu/libpcl_segmentation.so
+    /usr/lib/x86_64-linux-gnu/libpcl_surface.so
+    /usr/lib/x86_64-linux-gnu/libqhull.so
+    /opt/ros/melodic/lib/libnodeletlib.so
+    /opt/ros/melodic/lib/libbondcpp.so
+    /usr/lib/x86_64-linux-gnu/libuuid.so
+    /opt/ros/melodic/lib/librosbag.so
+    /opt/ros/melodic/lib/librosbag_storage.so
+#    /opt/ros/melodic/lib/libclass_loader.so
+    /usr/lib/libPocoFoundation.so
+    /opt/ros/melodic/lib/libroslib.so
+    /opt/ros/melodic/lib/librospack.so
+    /usr/lib/x86_64-linux-gnu/libboost_program_options.so
+    /usr/lib/x86_64-linux-gnu/libtinyxml2.so
+    /opt/ros/melodic/lib/libroslz4.so
+    /usr/lib/x86_64-linux-gnu/liblz4.so
+    /opt/ros/melodic/lib/libtopic_tools.so
+    /opt/ros/melodic/lib/libtf.so
+    /opt/ros/melodic/lib/libtf2_ros.so
+    /opt/ros/melodic/lib/libactionlib.so
+    /opt/ros/melodic/lib/libmessage_filters.so
+    /opt/ros/melodic/lib/libtf2.so
+    /usr/lib/x86_64-linux-gnu/libpcl_common.so
+    /usr/lib/x86_64-linux-gnu/libpcl_octree.so
+    /usr/lib/x86_64-linux-gnu/libpcl_io.so
+    /usr/lib/x86_64-linux-gnu/libboost_iostreams.so
+    /usr/lib/x86_64-linux-gnu/libboost_serialization.so
+    /usr/lib/libOpenNI.so
+    /usr/lib/libOpenNI2.so
+    /usr/lib/x86_64-linux-gnu/libfreetype.so
+    /usr/lib/x86_64-linux-gnu/libz.so
+    /usr/lib/x86_64-linux-gnu/libexpat.so
+    /usr/lib/x86_64-linux-gnu/libpython2.7.so
+    /usr/lib/x86_64-linux-gnu/libjpeg.so
+    /usr/lib/x86_64-linux-gnu/libpng.so
+    /usr/lib/x86_64-linux-gnu/libtiff.so
+    /usr/lib/x86_64-linux-gnu/libsqlite3.so
+    /usr/lib/x86_64-linux-gnu/libproj.so
+    /usr/lib/x86_64-linux-gnu/hdf5/openmpi/libhdf5.so
+    /usr/lib/x86_64-linux-gnu/libsz.so
+    /usr/lib/x86_64-linux-gnu/libdl.so
+    /usr/lib/x86_64-linux-gnu/libm.so
+    /usr/lib/x86_64-linux-gnu/openmpi/lib/libmpi.so
+    /usr/lib/x86_64-linux-gnu/libnetcdf_c++.so
+    /usr/lib/x86_64-linux-gnu/libnetcdf.so
+    /usr/lib/x86_64-linux-gnu/libtheoraenc.so
+    /usr/lib/x86_64-linux-gnu/libtheoradec.so
+    /usr/lib/x86_64-linux-gnu/libogg.so
+    /usr/lib/x86_64-linux-gnu/libxml2.so
+    /usr/lib/x86_64-linux-gnu/libjsoncpp.so
+    /opt/ros/melodic/lib/libroscpp.so
+    /usr/lib/x86_64-linux-gnu/libboost_filesystem.so
+    /opt/ros/melodic/lib/librosconsole.so
+    /opt/ros/melodic/lib/librosconsole_log4cxx.so
+    /opt/ros/melodic/lib/librosconsole_backend_interface.so
+    /usr/lib/x86_64-linux-gnu/liblog4cxx.so
+    /usr/lib/x86_64-linux-gnu/libboost_regex.so
+    /opt/ros/melodic/lib/libxmlrpcpp.so
+    /opt/ros/melodic/lib/libroscpp_serialization.so
+    /opt/ros/melodic/lib/librostime.so
+    /opt/ros/melodic/lib/libcpp_common.so
+    /usr/lib/x86_64-linux-gnu/libboost_system.so
+    /usr/lib/x86_64-linux-gnu/libboost_thread.so
+    /usr/lib/x86_64-linux-gnu/libboost_chrono.so
+    /usr/lib/x86_64-linux-gnu/libboost_date_time.so
+    /usr/lib/x86_64-linux-gnu/libboost_atomic.so
+    /usr/lib/x86_64-linux-gnu/libpthread.so
+    /usr/lib/x86_64-linux-gnu/libconsole_bridge.so )
+
+target_link_libraries(lidar
+    ${PCL_LIBRARIES}
+    io
+    utils
+    ${Boost_LIBRARIES}
+    ${catkin_LIBRARIES}
+    libclass_loader.so)

libs/lidar/abstract_parser.cpp

+#include "abstract_parser.h"
+
+AbstractParser::AbstractParser()
+{
+
+}
+
+bool checkLidarPosition(const uint8_t &devicePosition){
+    if(devicePosition < 0 || devicePosition > 1){
+        LOG(WARNING) << "invalid lidar_position: " << devicePosition;
+        return false;
+    }else{
+        return true;
+    }
+}

libs/lidar/abstract_parser.h

+#ifndef ABSTRACTPARSER_H
+#define ABSTRACTPARSER_H
+#pragma once
+
+#include "utils/basic_functions.h"
+#include "pcl_point_type.h"
+
+bool checkLidarPosition(const uint8_t &devicePosition);
+
+class AbstractParser
+{
+public:
+    AbstractParser();
+};
+
+#endif // ABSTRACTPARSER_H

libs/lidar/abstract_rotaryparser.cpp

+#include "abstract_rotaryparser.h"
+
+AbstractRotaryParser::AbstractRotaryParser()
+{
+
+}
+
+AbstractRotaryParser::AbstractRotaryParser(
+        const DevicePosition &LiDARPos,
+        const vector<PANDARLaserCorrection> &laserCorrection,
+        const vector<vector<bool> > &mask)
+    :LiDARPos_(LiDARPos),
+      laserCorrection_(laserCorrection),
+      mask_(mask)
+{
+    frame_.reset(new PointCloud);
+}
+
+PointCloud* AbstractRotaryParser::getFrame()
+{
+    PointCloud* ret(new PointCloud);
+    *ret = *frame_;
+    frame_.reset(new PointCloud);
+    return ret;
+}
+
+double AbstractRotaryParser::parseDataTime(const uint8_t *dataTime)
+{
+    tm t;
+    // 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 add 100 year.
+
+    t.tm_year = dataTime[0] >= 100 ? dataTime[0] : dataTime[0] + 100;
+    // UTC's month start from 1, but mktime only accept month from 0.
+    t.tm_mon = dataTime[1] - 1;
+    t.tm_mday = dataTime[2];
+    t.tm_hour = dataTime[3];
+    t.tm_min = dataTime[4];
+    t.tm_sec = dataTime[5];
+    t.tm_isdst = 0;
+    t.tm_wday = 0;
+    t.tm_yday = 0;
+    t.tm_zone = 0;
+    t.tm_gmtoff = 0;
+    double dataTimeSec = (double)mktime(&t);
+//    LOG(INFO) << setprecision(15) << "dataTime[0]: " << (int)dataTime[0] << endl
+//              << "t.tm_year: " << t.tm_year << endl
+//              << " t.tm_mon: " << t.tm_mon << endl
+//              << " t.tm_mday: " << t.tm_mday << endl
+//              << " t.tm_hour: " << t.tm_hour << endl
+//              << " t.tm_min: " << t.tm_min << endl
+//              << " t.tm_sec: " << t.tm_sec << endl
+//              << " dataTimeSec: " << dataTimeSec;
+//    sleep(1);
+    return dataTimeSec;
+}
+
+template<typename Packet>
+double AbstractRotaryParser::getPacketTimestamp(const unsigned char *data,
+                                        size_t bytesReceived)
+{
+//    if(bytesReceived != sizeof(Packet))
+//    {
+//        LOG_EVERY_N(WARNING, 99999)
+//                << "bytesReceived: " << bytesReceived << " != " << sizeof(Packet);
+//        return false;
+//    }
+    Packet* packet = const_cast<Packet*>(reinterpret_cast<const Packet*>(data));
+    return getPacketTimestamp<Packet>(packet);
+}
+
+template<typename Packet>
+double AbstractRotaryParser::getPacketTimestamp(const Packet *packet)
+{
+    double dataTimeSec = this->parseDataTime(packet->getDataTime());
+    double packetTimestamp = dataTimeSec + (double)packet->getUsec() / 1000000.0;
+//    LOG(INFO) << setprecision(15)
+//              << "packet->getDataTime(): " << packet->getDataTime()
+//              << " dataTimeSec: " << dataTimeSec
+//              << " packetTimestamp: " << packetTimestamp;
+    return packetTimestamp;
+}
+
+template<typename Packet, typename Block, typename Laser>
+bool AbstractRotaryParser::ParsePacket(const unsigned char *data)
+{
+    const Packet* packet = reinterpret_cast<const Packet*>(data);
+    double packetTime = getPacketTimestamp<Packet>(packet);
+    bool ret = false;
+    for (unsigned int firingBlock = 0; firingBlock < blockPerPacket_; ++firingBlock)
+    {
+        const Block* firingData = &(packet->blockData[firingBlock]);
+        if (firingData->azimuth < lastAzimuth){
+            ret = true;
+        }
+        ParseBlock<Block, Laser>(firingData, firingBlock, packetTime);
+        lastAzimuth = firingData->azimuth;
+    }
+    return ret;
+}
+
+template<typename Block, typename Laser>
+void AbstractRotaryParser::ParseBlock(
+        const Block *firingData,
+        const uint8_t &firingBlock,
+        const double &timestamp)
+{
+    for (uint8_t laserid = 0; laserid < laserPerBlock_; laserid++)
+    {
+        uint16_t azimuth = firingData->azimuth;
+        if(!checkPoint(laserid, azimuth)){
+            continue;
+        }
+        double azimuthadjustment = laserCorrection_[laserid].azimuthCorrection * 100;
+        double timestampadjustment = ((double)(blockOffset_[firingBlock] + (double)laserOffset_[laserid]) / 1000000.0f);
+        ParsePoint<Laser>(&(firingData->laserReturns[laserid]),
+                          laserid,
+                          (int32_t)(10 * (azimuth + azimuthadjustment)),
+                          timestamp + timestampadjustment);
+    }
+}
+
+template<typename Laser>
+void AbstractRotaryParser::ParsePoint(
+        const Laser* laserReturn,
+        const uint8_t &laserId,
+        int32_t azimuth,
+        const double &timestamp)
+{
+    while(azimuth < 0){
+        azimuth += 360000;
+    }
+    azimuth %= 360000;
+    double distanceM = laserReturn->distance * distanceResolution_;
+    const uint8_t intensity = laserReturn->intensity;
+    double cosAzimuth = 0, sinAzimuth = 0;
+    cosAzimuth = horizon_cos_lookup_table_[azimuth];
+    sinAzimuth = horizon_sin_lookup_table_[azimuth];
+    double xyDistance = distanceM * vertical_cos_lookup_table_[laserId];
+    Point p;
+    p.x = xyDistance * sinAzimuth;
+    p.y = xyDistance * cosAzimuth;
+    p.z = distanceM * vertical_sin_lookup_table_[laserId];
+    if(distanceM < 0.01 && vertical_sin_lookup_table_[laserId] > 0){
+        distanceM = 200;
+    }
+    p.distance = distanceM;
+    p.intensity = intensity;
+    p.ring = laserId;
+    p.timestamp = timestamp;
+    frame_->points.emplace_back(p);
+}
+
+void AbstractRotaryParser::InitTables()
+{
+    if (horizon_cos_lookup_table_.size() == 0 || horizon_sin_lookup_table_.size() == 0)
+    {
+        horizon_cos_lookup_table_.resize(blockPerFrame_);
+        horizon_sin_lookup_table_.resize(blockPerFrame_);
+        for (unsigned int i = 0; i < blockPerFrame_; i++)
+        {
+            double rad = ToRadians(i / 1000.0);
+            horizon_cos_lookup_table_[i] = cos(rad);
+            horizon_sin_lookup_table_[i] = sin(rad);
+        }
+    }
+
+    if (vertical_cos_lookup_table_.size() == 0 || vertical_sin_lookup_table_.size() == 0)
+    {
+        vertical_cos_lookup_table_.resize(laserPerBlock_);
+        vertical_sin_lookup_table_.resize(laserPerBlock_);
+        for (unsigned int laserId = 0; laserId < laserPerBlock_; laserId++)
+        {
+            double rad = ToRadians(laserCorrection_[laserId].verticalCorrection);
+            vertical_cos_lookup_table_[laserId] = cos(rad);
+            vertical_sin_lookup_table_[laserId] = sin(rad);
+        }
+    }
+}
+
+bool AbstractRotaryParser::checkPoint(const uint8_t laserId,
+                                      const int32_t azimuth)
+{
+    if(mask_.size() > laserId){
+        if(mask_[laserId].size() > (azimuth / 10)){
+            return mask_[laserId][azimuth / 10];
+        }
+    }
+    return true;
+}
+
+template bool AbstractRotaryParser::ParsePacket
+<PANDAR40DataPacket, PANDAR40BlockData, PANDAR40LaserReturn>(const unsigned char *data);
+template bool AbstractRotaryParser::ParsePacket
+<PANDARXT32DataPacket, PANDARXT32BlockData, PANDARXT32LaserReturn>(const unsigned char *data);
+template bool AbstractRotaryParser::ParsePacket
+<PANDAR80DataPacket, PANDAR80BlockData, PANDAR80LaserReturn>(const unsigned char *data);
+template bool AbstractRotaryParser::ParsePacket
+<PANDAR64DataPacket, PANDAR64BlockData, PANDAR64LaserReturn>(const unsigned char *data);
+
+template void AbstractRotaryParser::ParseBlock
+<PANDAR40BlockData, PANDAR40LaserReturn>(
+const PANDAR40BlockData *firingData,
+const uint8_t &firingBlock,
+const double &timestamp);
+template void AbstractRotaryParser::ParseBlock
+<PANDARXT32BlockData, PANDARXT32LaserReturn>(
+const PANDARXT32BlockData *firingData,
+const uint8_t &firingBlock,
+const double &timestamp);
+template void AbstractRotaryParser::ParseBlock
+<PANDAR80BlockData, PANDAR80LaserReturn>(
+const PANDAR80BlockData *firingData,
+const uint8_t &firingBlock,
+const double &timestamp);
+template void AbstractRotaryParser::ParseBlock
+<PANDAR64BlockData, PANDAR64LaserReturn>(
+const PANDAR64BlockData *firingData,
+const uint8_t &firingBlock,
+const double &timestamp);
+
+template void AbstractRotaryParser::ParsePoint
+<PANDAR40LaserReturn>(
+const PANDAR40LaserReturn* laserReturn,
+const uint8_t &laserId,
+int32_t azimuth,
+const double &timestamp);
+template void AbstractRotaryParser::ParsePoint
+<PANDARXT32LaserReturn>(
+const PANDARXT32LaserReturn* laserReturn,
+const uint8_t &laserId,
+int32_t azimuth,
+const double &timestamp);
+template void AbstractRotaryParser::ParsePoint
+<PANDAR80LaserReturn>(
+const PANDAR80LaserReturn* laserReturn,
+const uint8_t &laserId,
+int32_t azimuth,
+const double &timestamp);
+template void AbstractRotaryParser::ParsePoint
+<PANDAR64LaserReturn>(
+const PANDAR64LaserReturn* laserReturn,
+const uint8_t &laserId,
+int32_t azimuth,
+const double &timestamp);
+
+template double AbstractRotaryParser::getPacketTimestamp
+<PANDAR40DataPacket>(const unsigned char *data, size_t bytesReceived);
+template double AbstractRotaryParser::getPacketTimestamp
+<PANDARXT32DataPacket>(const unsigned char *data, size_t bytesReceived);
+template double AbstractRotaryParser::getPacketTimestamp
+<PANDAR80DataPacket>(const unsigned char *data, size_t bytesReceived);
+template double AbstractRotaryParser::getPacketTimestamp
+<PANDAR64DataPacket>(const unsigned char *data, size_t bytesReceived);
+
+template double AbstractRotaryParser::getPacketTimestamp
+<PANDAR40DataPacket>(const PANDAR40DataPacket *packet);
+template double AbstractRotaryParser::getPacketTimestamp
+<PANDARXT32DataPacket>(const PANDARXT32DataPacket *packet);
+template double AbstractRotaryParser::getPacketTimestamp
+<PANDAR80DataPacket>(const PANDAR80DataPacket *packet);
+template double AbstractRotaryParser::getPacketTimestamp
+<PANDAR64DataPacket>(const PANDAR64DataPacket *packet);

libs/lidar/abstract_rotaryparser.h

+#ifndef ABSTRACT_ROTARYPARSER_H
+#define ABSTRACT_ROTARYPARSER_H
+#include "abstract_parser.h"
+#include <boost/function.hpp>
+#include "lidar_defines.h"
+
+class AbstractRotaryParser : public AbstractParser
+{
+public:
+    AbstractRotaryParser();
+
+    AbstractRotaryParser(const DevicePosition &LiDARPos,
+                         const vector<PANDARLaserCorrection> &laserCorrection,
+                         const  vector<vector<bool>> &mask);
+
+    PointCloud *getFrame();
+
+    virtual bool ParsePacket(const unsigned char *data) = 0;
+
+    virtual double getPacketTimestamp(
+            const unsigned char *data,
+            size_t bytesReceived) = 0;
+
+protected:
+    virtual void Init() = 0;
+
+    void InitTables();
+
+    virtual void setParams() = 0;
+
+    template<typename Packet>
+    double getPacketTimestamp(const unsigned char *data,
+                              size_t bytesReceived);
+
+    template<typename Packet, typename Block, typename Laser>
+    bool ParsePacket(const unsigned char *data);
+
+private:
+    double parseDataTime(const uint8_t *dataTime);
+
+    template<typename Block, typename Laser>
+    void ParseBlock(const Block *firingData,
+                     const uint8_t &firingBlock,
+                     const double &timestamp);
+
+    template<typename Laser>
+    void ParsePoint(const Laser *laserReturn,
+                    const uint8_t &laserId,
+                    int32_t azimuth,
+                    const double &timestamp);
+
+    template<typename Packet>
+    double getPacketTimestamp(const Packet *packet);
+
+    bool checkPoint(const uint8_t laserId, const int32_t azimuth);
+protected:
+    PointCloud::Ptr frame_;
+
+    DevicePosition LiDARPos_;
+
+    vector<double> horizon_cos_lookup_table_;
+
+    vector<double> horizon_sin_lookup_table_;
+
+    vector<double> vertical_cos_lookup_table_;
+
+    vector<double> vertical_sin_lookup_table_;
+
+    uint16_t lastAzimuth = 0;
+
+    vector<PANDARLaserCorrection> laserCorrection_;
+
+    vector<vector<bool>> mask_;
+
+    uint32_t blockPerPacket_ = 0;
+
+    uint32_t laserPerBlock_ = 0;
+
+    vector<float> blockOffset_;
+
+    vector<float> laserOffset_;
+
+    float distanceResolution_ = 0;
+
+    uint32_t blockPerFrame_ = 0;
+};
+
+#endif // ABSTRACT_ROTARYPARSER_H

libs/lidar/lidar_defines.h

+#ifndef LIDAR_DEFINES_H
+#define LIDAR_DEFINES_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+#define ToRadians(x) ((x) * M_PI / 180.0)
+
+#pragma pack(push, 1)
+struct PANDARLaserCorrection
+{
+    double azimuthCorrection;
+    double verticalCorrection;
+    double distanceCorrection;
+    double verticalOffsetCorrection;
+    double horizontalOffsetCorrection;
+    double sinVertCorrection;
+    double cosVertCorrection;
+    double sinVertOffsetCorrection;
+    double cosVertOffsetCorrection;
+};
+#pragma pack(pop)
+
+const size_t  HesaiP40_Packet_Length = 1262;
+const size_t  HesaiXT32_Packet_Length = 1080;
+const size_t  Hesai80_Packet_Length = 1409;
+const size_t  Hesai64_Packet_Length = 1194;
+
+#define PANDAR_XT32_NUM_ROT_ANGLES (360001)
+#define PANDAR_XT32_LASER_PER_BLOCK (32)
+#define PANDAR_XT32_BLOCK_PER_PKT (8)
+#define PANDAR_XT32_MAX_NUM_LASERS (64)
+#define PANDAR_XT32_DISTANCE_RESOLUTION (0.004)
+
+#pragma pack(push, 1)
+struct PANDARXT32PreHeader
+{
+    uint16_t blockIdentifier;
+    uint16_t version;
+    uint16_t reserved;
+};
+
+struct PANDARXT32Header
+{
+    uint8_t laserNum;
+    uint8_t blockNum;
+    uint8_t fstBlockReturn;
+    uint8_t disUnit;
+    uint8_t returnNumber;
+    uint8_t UDPSeq;
+};
+
+struct PANDARXT32LaserReturn
+{
+    uint16_t distance;
+    uint8_t intensity;
+    uint8_t reserved;
+};
+
+struct PANDARXT32BlockData
+{
+    uint16_t azimuth;
+    PANDARXT32LaserReturn laserReturns[PANDAR_XT32_LASER_PER_BLOCK];
+};
+
+struct PANDARXT32Tail
+{
+    uint8_t reserved[10];
+    uint8_t returnMode;
+    uint8_t motorSpeed[2];
+    uint8_t dataTime[6];
+    uint32_t usec;
+    uint8_t factoryInfo;
+};
+
+struct PANDARXT32AddiInfo
+{
+    uint32_t UDPSequence;
+};
+
+struct PANDARXT32DataPacket
+{
+    PANDARXT32PreHeader preHeader;
+    PANDARXT32Header header;
+    PANDARXT32BlockData blockData[PANDAR_XT32_BLOCK_PER_PKT];
+    PANDARXT32Tail tail;
+    PANDARXT32AddiInfo addiInfo;
+    const uint8_t* getDataTime()const {
+        return tail.dataTime;
+    }
+    const uint32_t getUsec()const {
+        return tail.usec;
+    }
+};
+
+#pragma pack(pop)
+
+
+#define PANDAR_40_NUM_ROT_ANGLES (360001)
+#define PANDAR_40_LASER_PER_BLOCK (40)
+#define PANDAR_40_BLOCK_PER_PKT (10)
+#define PANDAR_40_MAX_NUM_LASERS (64)
+#define PANDAR_40_DISTANCE_RESOLUTION (0.004)
+
+#pragma pack(push, 1)
+struct PANDAR40LaserReturn
+{
+    uint16_t distance;
+    uint8_t intensity;
+};
+
+struct PANDAR40BlockData
+{
+    uint16_t blockIdentifier;
+    uint16_t azimuth;
+    PANDAR40LaserReturn laserReturns[PANDAR_40_LASER_PER_BLOCK];
+};
+
+struct PANDAR40DataPacket
+{
+    PANDAR40BlockData blockData[PANDAR_40_BLOCK_PER_PKT];
+    uint8_t reserved[10];
+    uint32_t usec;
+    uint8_t returnMode;
+    uint8_t blank;
+    uint8_t dataTime[6];
+    const uint8_t* getDataTime()const {
+        return dataTime;
+    }
+    const uint32_t getUsec()const {
+        return usec;
+    }
+};
+
+#pragma pack(pop)
+
+#define PANDAR_80_NUM_ROT_ANGLES (360001)
+#define PANDAR_80_LASER_PER_BLOCK (80)
+#define PANDAR_80_BLOCK_PER_PKT (4)
+#define PANDAR_80_MAX_NUM_LASERS (84)
+#define PANDAR_80_DISTANCE_RESOLUTION (0.004)
+
+#pragma pack(push, 1)
+
+struct PANDAR80PreHeader
+{
+    uint16_t blockIdentifier;
+    uint16_t version;
+    uint16_t reserved;
+};
+
+struct PANDAR80Header
+{
+    uint8_t laserNum;
+    uint8_t blockNum;
+    uint8_t fstBlockReturn;
+    uint8_t disUnit;
+    uint8_t returnNumber;
+    uint8_t UDPSeq;
+};
+
+struct PANDAR80LaserReturn
+{
+    uint16_t distance;
+    uint8_t intensity;
+    uint8_t reserved;
+};
+
+struct PANDAR80BlockData
+{
+    uint16_t azimuth;
+    PANDAR80LaserReturn laserReturns[PANDAR_80_LASER_PER_BLOCK];
+};
+
+struct PANDAR80Safety
+{
+    uint8_t reserved[17];
+};
+
+struct PANDAR80Tail
+{
+    uint8_t reserved_1[12];
+    uint8_t returnMode;
+    uint8_t motorSpeed[2];
+    uint8_t dataTime[6];
+    uint32_t usec;
+    uint8_t reserved_2[31];
+};
+
+struct PANDAR80Signature
+{
+    uint8_t signature[32];
+};
+
+struct PANDAR80DataPacket
+{
+    PANDAR80PreHeader preHeader;
+    PANDAR80Header header;
+    PANDAR80BlockData blockData[PANDAR_80_BLOCK_PER_PKT];
+    uint32_t crc;
+    PANDAR80Safety safety;
+    PANDAR80Tail tail;
+    PANDAR80Signature signature;
+    const uint8_t* getDataTime()const {
+        return tail.dataTime;
+    }
+    const uint32_t getUsec()const {
+        return tail.usec;
+    }
+};
+
+#pragma pack(pop)
+
+#define PANDAR_64_NUM_ROT_ANGLES (360001)
+#define PANDAR_64_LASER_PER_BLOCK (64)
+#define PANDAR_64_BLOCK_PER_PKT (6)
+#define PANDAR_64_MAX_NUM_LASERS (84)
+#define PANDAR_64_DISTANCE_RESOLUTION (0.004)
+
+#pragma pack(push, 1)
+
+struct PANDAR64Header
+{
+    uint16_t blockIdentifier;
+    uint8_t laserNum;
+    uint8_t blockNum;
+    uint8_t reserved;
+    uint8_t disUnit;
+    uint16_t reserved_2;
+};
+
+struct PANDAR64LaserReturn
+{
+    uint16_t distance;
+    uint8_t intensity;
+};
+
+struct PANDAR64BlockData
+{
+    uint16_t azimuth;
+    PANDAR64LaserReturn laserReturns[PANDAR_64_LASER_PER_BLOCK];
+};
+
+struct PANDAR64Tail
+{
+    uint8_t reserved_1[10];
+    uint32_t usec;
+    uint8_t returnMode;
+    uint8_t reserved_2;
+    uint8_t dataTime[6];
+    uint8_t reserved_3[4];
+};
+
+struct PANDAR64DataPacket
+{
+    PANDAR64Header header;
+    PANDAR64BlockData blockData[PANDAR_64_BLOCK_PER_PKT];
+    PANDAR64Tail tail;
+    const uint8_t* getDataTime()const {
+        return tail.dataTime;
+    }
+    const uint32_t getUsec()const {
+        return tail.usec;
+    }
+};
+
+#pragma pack(pop)
+
+#endif // LIDAR_DEFINES_H

libs/lidar/pandar40_parser.cpp

+#include "pandar40_parser.h"
+
+#include <boost/foreach.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+#include <boost/filesystem.hpp>
+
+
+Pandar40Parser::Pandar40Parser()
+    :AbstractRotaryParser()
+{
+
+}
+
+Pandar40Parser::Pandar40Parser(const DevicePosition &LiDARPos,
+                               const vector<PANDARLaserCorrection> &laserCorrection,
+                               const vector<vector<bool> > &mask)
+    :AbstractRotaryParser(LiDARPos, laserCorrection, mask)
+{
+    setParams();
+    InitTables();
+    Init();
+}
+
+Pandar40Parser::~Pandar40Parser()
+{
+
+}
+
+bool Pandar40Parser::ParsePacket(const unsigned char *data)
+{
+    return AbstractRotaryParser::ParsePacket<PANDAR40DataPacket, PANDAR40BlockData, PANDAR40LaserReturn>(data);
+}
+
+double Pandar40Parser::getPacketTimestamp(const unsigned char *data, size_t bytesReceived)
+{
+    return AbstractRotaryParser::getPacketTimestamp<PANDAR40DataPacket>(data, bytesReceived);
+}
+
+void Pandar40Parser::Init()
+{
+    // init the block time offset, us
+    ///40P timeadjustment take minus!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+    ///
+    blockOffset_[9] = -55.56f * 0.0f - 28.58f;
+    blockOffset_[8] = -55.56f * 1.0f - 28.58f;
+    blockOffset_[7] = -55.56f * 2.0f - 28.58f;
+    blockOffset_[6] = -55.56f * 3.0f - 28.58f;
+    blockOffset_[5] = -55.56f * 4.0f - 28.58f;
+    blockOffset_[4] = -55.56f * 5.0f - 28.58f;
+    blockOffset_[3] = -55.56f * 6.0f - 28.58f;
+    blockOffset_[2] = -55.56f * 7.0f - 28.58f;
+    blockOffset_[1] = -55.56f * 8.0f - 28.58f;
+    blockOffset_[0] = -55.56f * 9.0f - 28.58f;
+
+    laserOffset_[3] = -3.62f;
+    laserOffset_[39] = -3.62f;
+    laserOffset_[35] = -4.92f;
+    laserOffset_[27] = -6.23f;
+    laserOffset_[11] = -8.19f;
+    laserOffset_[15] = -8.19f;
+    laserOffset_[31] = -9.5f;
+    laserOffset_[23] = -11.47f;
+    laserOffset_[28] = -12.77f;
+    laserOffset_[16] = -14.74f;
+    laserOffset_[2] = -16.04f;
+    laserOffset_[38] = -16.04f;
+    laserOffset_[34] = -17.35f;
+    laserOffset_[24] = -18.65f;
+    laserOffset_[8] = -20.62f;
+    laserOffset_[12] = -20.62f;
+    laserOffset_[30] = -21.92f;
+    laserOffset_[20] = -23.89f;
+    laserOffset_[25] = -25.19f;
+    laserOffset_[13] = -27.16f;
+    laserOffset_[1] = -28.47f;
+    laserOffset_[37] = -28.47f;
+    laserOffset_[33] = -29.77f;
+    laserOffset_[5] = -31.74f;
+    laserOffset_[21] = -31.7447f;
+    laserOffset_[9] = -33.71f;
+    laserOffset_[29] = -35.01f;
+    laserOffset_[17] = -36.98f;
+    laserOffset_[22] = -38.95f;
+    laserOffset_[10] = -40.91f;
+    laserOffset_[0] = -42.22f;
+    laserOffset_[36] = -42.22f;
+    laserOffset_[32] = -43.52f;
+    laserOffset_[4] = -45.49f;
+    laserOffset_[18] = -45.49f;
+    laserOffset_[6] = -47.46f;
+    laserOffset_[26] = -48.76f;
+    laserOffset_[14] = -50.73f;
+    laserOffset_[19] = -52.7f;
+    laserOffset_[7] = -54.67f;
+
+}
+
+void Pandar40Parser::setParams()
+{
+    blockPerPacket_ = PANDAR_40_BLOCK_PER_PKT;
+    laserPerBlock_ = PANDAR_40_LASER_PER_BLOCK;
+    blockOffset_.resize(blockPerPacket_);
+    laserOffset_.resize(laserPerBlock_);
+    distanceResolution_ = PANDAR_40_DISTANCE_RESOLUTION;
+    blockPerFrame_ = PANDAR_40_NUM_ROT_ANGLES;
+}

libs/lidar/pandar40_parser.h

+#ifndef PANDAR40PARSER_H
+#define PANDAR40PARSER_H
+#include "abstract_rotaryparser.h"
+
+class Pandar40Parser : public AbstractRotaryParser
+{
+public:
+    Pandar40Parser();
+
+    Pandar40Parser(const DevicePosition &LiDARPos,
+                   const vector<PANDARLaserCorrection> &laserCorrection,
+                   const vector<vector<bool> > &mask);
+
+    ~Pandar40Parser();
+
+    bool ParsePacket(const unsigned char *data);
+
+    double getPacketTimestamp(
+            const unsigned char *data,
+            size_t bytesReceived);
+private:
+    void Init();
+
+    void setParams();
+
+};
+
+#endif // PANDAR40PARSER_H

libs/lidar/pandar64parser.cpp

+#include "pandar64parser.h"
+#include <boost/foreach.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+#include <boost/filesystem.hpp>
+
+Pandar64Parser::Pandar64Parser()
+    :AbstractRotaryParser()
+{
+
+}
+
+Pandar64Parser::Pandar64Parser(const DevicePosition &LiDARPos,
+                               const vector<PANDARLaserCorrection> &laserCorrection,
+                               const vector<vector<bool> > &mask)
+    :AbstractRotaryParser(LiDARPos, laserCorrection, mask)
+{
+    setParams();
+    InitTables();
+    Init();
+}
+
+Pandar64Parser::~Pandar64Parser()
+{
+
+}
+
+bool Pandar64Parser::ParsePacket(const unsigned char *data)
+{
+    return AbstractRotaryParser::ParsePacket<PANDAR64DataPacket, PANDAR64BlockData, PANDAR64LaserReturn>(data);
+}
+
+double Pandar64Parser::getPacketTimestamp(const unsigned char *data, size_t bytesReceived)
+{
+    return AbstractRotaryParser::getPacketTimestamp<PANDAR64DataPacket>(data, bytesReceived);
+}
+
+void Pandar64Parser::Init()
+{
+    // init the block time offset, us
+    for(size_t i = 0; i < PANDAR_64_BLOCK_PER_PKT; i++){
+        blockOffset_[i] = - 42.58 - 55.56 * (6 - (int32_t)i);
+    }
+}
+
+void Pandar64Parser::setParams()
+{
+    blockPerPacket_ = PANDAR_64_BLOCK_PER_PKT;
+    laserPerBlock_ = PANDAR_64_LASER_PER_BLOCK;
+    blockOffset_.resize(blockPerPacket_);
+    laserOffset_.resize(laserPerBlock_, 0);
+    distanceResolution_ = PANDAR_64_DISTANCE_RESOLUTION;
+    blockPerFrame_ = PANDAR_64_NUM_ROT_ANGLES;
+}

libs/lidar/pandar64parser.h

+#ifndef PANDAR64PARSER_H
+#define PANDAR64PARSER_H
+#include "abstract_rotaryparser.h"
+
+
+class Pandar64Parser : public AbstractRotaryParser
+{
+public:
+    Pandar64Parser();
+
+    Pandar64Parser(const DevicePosition &LiDARPos,
+                   const vector<PANDARLaserCorrection> &laserCorrection,
+                   const vector<vector<bool> > &mask);
+    ~Pandar64Parser();
+
+    bool ParsePacket(const unsigned char *data);
+
+    double getPacketTimestamp(
+            const unsigned char *data,
+            size_t bytesReceived);
+private:
+    void Init();
+
+    void setParams();
+};
+
+#endif // PANDAR64PARSER_H

libs/lidar/pandar80_parser.cpp

+#include "pandar80_parser.h"
+
+#include <boost/foreach.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+#include <boost/filesystem.hpp>
+
+
+Pandar80Parser::Pandar80Parser()
+    :AbstractRotaryParser()
+{
+
+}
+
+Pandar80Parser::Pandar80Parser(const DevicePosition &LiDARPos,
+                               const vector<PANDARLaserCorrection> &laserCorrection,
+                               const vector<vector<bool> > &mask)
+    :AbstractRotaryParser(LiDARPos, laserCorrection, mask)
+{
+    setParams();
+    InitTables();
+    Init();
+}
+
+Pandar80Parser::~Pandar80Parser()
+{
+
+}
+
+bool Pandar80Parser::ParsePacket(const unsigned char *data)
+{
+    return AbstractRotaryParser::ParsePacket<PANDAR80DataPacket, PANDAR80BlockData, PANDAR80LaserReturn>(data);
+}
+
+double Pandar80Parser::getPacketTimestamp(const unsigned char *data, size_t bytesReceived)
+{
+    return AbstractRotaryParser::getPacketTimestamp<PANDAR80DataPacket>(data, bytesReceived);
+}
+
+void Pandar80Parser::Init()
+{
+    // init the block time offset, us
+    for(size_t i = 0; i < PANDAR_80_BLOCK_PER_PKT; i++){
+        blockOffset_[i] = 3.148 + 33.333 * ((int32_t)i - 3);
+    }
+
+    for(size_t i = 0; i < 16; i++){
+        float timeOffset = 0.776 + i * 1.91;
+        if(i < 8){
+            laserOffset_[3 + 2 * i - 1] = timeOffset;
+            laserOffset_[19 + 2 * i - 1] = timeOffset;
+            laserOffset_[32 + 2 * i - 1] = timeOffset;
+            laserOffset_[48 + 2 * i - 1] = timeOffset;
+            laserOffset_[64 + 2 * i - 1] = timeOffset;
+        }else if(i == 8){
+            laserOffset_[0] = timeOffset;
+            laserOffset_[15] = timeOffset;
+            laserOffset_[34] = timeOffset;
+            laserOffset_[50] = timeOffset;
+            laserOffset_[66] = timeOffset;
+        }else{
+            laserOffset_[2 + 2 * (i - 9)- 1] = timeOffset;
+            laserOffset_[18 + 2 * (i - 9)- 1] = timeOffset;
+            laserOffset_[37 + 2 * (i - 9)- 1] = timeOffset;
+            laserOffset_[53 + 2 * (i - 9)- 1] = timeOffset;
+            if(i == 15){
+                laserOffset_[79] = timeOffset;
+            }else{
+                laserOffset_[69 + 2 * (i - 9)- 1] = timeOffset;
+            }
+        }
+    }
+    for(size_t i = 0; i < 80; i++){
+        if(laserOffset_[i] == 0){
+            LOG(WARNING) << "laserOffset_[i] == 0! i = " << i;
+        }
+    }
+}
+
+void Pandar80Parser::setParams()
+{
+    blockPerPacket_ = PANDAR_80_BLOCK_PER_PKT;
+    laserPerBlock_ = PANDAR_80_LASER_PER_BLOCK;
+    blockOffset_.resize(blockPerPacket_);
+    laserOffset_.resize(laserPerBlock_, 0);
+    distanceResolution_ = PANDAR_80_DISTANCE_RESOLUTION;
+    blockPerFrame_ = PANDAR_80_NUM_ROT_ANGLES;
+}

libs/lidar/pandar80_parser.h

+#ifndef PANDAR80PARSER_H
+#define PANDAR80PARSER_H
+#include "abstract_rotaryparser.h"
+
+class Pandar80Parser : public AbstractRotaryParser
+{
+public:
+    Pandar80Parser();
+
+    Pandar80Parser(const DevicePosition &LiDARPos,
+                   const vector<PANDARLaserCorrection> &laserCorrection,
+                   const vector<vector<bool> > &mask);
+
+    ~Pandar80Parser();
+
+    bool ParsePacket(const unsigned char *data);
+
+    double getPacketTimestamp(
+            const unsigned char *data,
+            size_t bytesReceived);
+private:
+    void Init();
+
+    void setParams();
+
+};
+
+#endif // PANDAR80PARSER_H

libs/lidar/pandarxt32_parser.cpp

+#include "pandarxt32_parser.h"
+
+#include <boost/foreach.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+#include <boost/filesystem.hpp>
+
+
+PandarXT32Parser::PandarXT32Parser()
+    :AbstractRotaryParser()
+{
+
+}
+
+PandarXT32Parser::PandarXT32Parser(const DevicePosition &LiDARPos,
+                                   const vector<PANDARLaserCorrection> &laserCorrection,
+                                   const vector<vector<bool> > &mask)
+    :AbstractRotaryParser(LiDARPos, laserCorrection, mask)
+{
+    setParams();
+    InitTables();
+    Init();
+}
+
+PandarXT32Parser::~PandarXT32Parser()
+{
+
+}
+
+bool PandarXT32Parser::ParsePacket(const unsigned char *data)
+{
+    return AbstractRotaryParser::ParsePacket<PANDARXT32DataPacket, PANDARXT32BlockData, PANDARXT32LaserReturn>(data);
+}
+
+double PandarXT32Parser::getPacketTimestamp(const unsigned char *data, size_t bytesReceived)
+{
+    return AbstractRotaryParser::getPacketTimestamp<PANDARXT32DataPacket>(data, bytesReceived);
+}
+
+void PandarXT32Parser::Init()
+{
+    // init the block time offset, us
+    for(size_t i = 0; i < PANDAR_XT32_BLOCK_PER_PKT; i++){
+        blockOffset_[i] = 3.28 - 50 * (7 - i);
+    }
+
+    for(size_t i = 0; i < PANDAR_XT32_LASER_PER_BLOCK; i++){
+        laserOffset_[i] = 1.512 * i + 0.28;
+    }
+
+}
+
+void PandarXT32Parser::setParams()
+{
+    blockPerPacket_ = PANDAR_XT32_BLOCK_PER_PKT;
+    laserPerBlock_ = PANDAR_XT32_LASER_PER_BLOCK;
+    blockOffset_.resize(blockPerPacket_);
+    laserOffset_.resize(laserPerBlock_);
+    distanceResolution_ = PANDAR_XT32_DISTANCE_RESOLUTION;
+    blockPerFrame_ = PANDAR_XT32_NUM_ROT_ANGLES;
+}

libs/lidar/pandarxt32_parser.h

+#ifndef PANDARXT32_PARSER_H
+#define PANDARXT32_PARSER_H
+#include "abstract_rotaryparser.h"
+
+class PandarXT32Parser : public AbstractRotaryParser
+{
+public:
+    PandarXT32Parser();
+
+    PandarXT32Parser(const DevicePosition &LiDARPos,
+                     const vector<PANDARLaserCorrection> &laserCorrection,
+                     const vector<vector<bool> > &mask);
+
+    ~PandarXT32Parser();
+
+    bool ParsePacket(const unsigned char *data);
+
+    double getPacketTimestamp(
+            const unsigned char *data,
+            size_t bytesReceived);
+private:
+    void Init();
+
+    void setParams();
+
+};
+
+#endif // PANDARXT32_PARSER_H

libs/lidar/pcl_point_type.h

+//#pragma once
+#define PCL_NO_PRECOMPILE
+#ifndef PCL_POINT_TYPE_H
+#define PCL_POINT_TYPE_H
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+namespace pcl {
+struct PointInternal {
+    PCL_ADD_POINT4D
+    PCL_ADD_RGB
+    float intensity;
+    float ring;
+    float label = 0;
+    float distance;
+    double timestamp;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+}//end namespace pcl
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    pcl::PointInternal, (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)
+        (float, intensity, intensity)(float, ring, ring)(float, label, label)
+        (float, distance, distance)(double, timestamp, timestamp))
+
+typedef pcl::PointInternal Point;
+typedef pcl::PointCloud<Point> PointCloud;
+
+/////////////////////////////////////////////////////////////
+///
+///
+namespace pcl {
+struct PointRos {
+    PCL_ADD_POINT4D
+    float intensity;
+    float ring;
+    float label = 0;
+    float timestamp;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN32;
+}//end namespace pcl
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    pcl::PointRos, (float, x, x)(float, y, y)(float, z, z)
+        (float, intensity, intensity)(float, ring, ring)(float, label, label)
+        (float, timestamp, timestamp))
+
+typedef pcl::PointRos PointRos;
+typedef pcl::PointCloud<PointRos> PointCloudRos;
+
+/////////////////////////////////////////////////////////////
+///
+///
+namespace pcl {
+struct PointExport {
+    PCL_ADD_POINT4D
+    PCL_ADD_RGB
+    float intensity = 0;
+    float label = 0;
+    float info = 0;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+}//end namespace pcl
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    pcl::PointExport, (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)
+        (float, intensity, intensity)(float, label, label)(float, info, info))
+
+typedef pcl::PointExport PointExport;
+typedef pcl::PointCloud<PointExport> PointCloudExport;
+
+/////////////////////////////////////////////////////////////
+namespace pcl {
+struct PointTemp1 {
+    PCL_ADD_POINT4D
+    float intensity;
+    float ring;
+    float label = 0;
+    float time;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+}//end namespace pcl
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    pcl::PointTemp1, (float, x, x)(float, y, y)(float, z, z)
+        (float, intensity, intensity)(float, ring, ring)(float, label, label)
+        (float, time, time))
+
+typedef pcl::PointTemp1 PointTemp1;
+
+
+#endif // PCL_POINT_TYPE_H

libs/mapping/CMakeLists.txt

+add_library(mapping STATIC map_builder.cpp)
+
+target_link_libraries(task
+    utils
+    lidar
+    reader
+    trajectory
+    ${Boost_LIBRARIES})

libs/params_control/CMakeLists.txt

+add_library(params_control STATIC params_server.cpp strategy_control.cpp)
+
+target_link_libraries(params_control
+    transform
+    yaml-cpp
+    )

libs/params_control/params_server.h

+#ifndef PARAMSSERVER_H
+#define PARAMSSERVER_H
+#include "io/PcapFileReader.h"
+#include "lidar/pandar40_parser.h"
+#include "lidar/pandar64parser.h"
+#include "lidar/pandar80_parser.h"
+#include "lidar/pandarxt32_parser.h"
+#include "trajectory/trajectory.h"
+#include "transform/transform.h"
+
+struct TaskComponent {
+  vector<string> taskIdVec;
+  boost::shared_ptr<Trajectory> mappingTraj;
+  boost::shared_ptr<Trajectory> undistortionTraj;
+  vector<vector<Time_Duration>> timeDurationsVec;
+  string outputPrefix;
+  bool merged = false;
+};
+
+struct TaskDescribe {
+  string task_prefix;
+  vector<TaskComponent> taskComponentVec;
+};
+
+class ParamsServer {
+ public:
+  ParamsServer(const string &taskPath);
+
+  string getTaskPath();
+
+  void load();
+
+  void initTrajBackend();
+
+  Matrix4d getDeviceCalib(const uint8_t &index) const;
+
+  vector<string> getPcapPath(const uint8_t &index) const;
+
+  vector<string> getBagPath(const uint8_t &index) const;
+
+  string getPcdDir() const { return pcdDir_; };
+
+  LidarType getLidarType(const uint8_t &index) const;
+
+  Matrix4d getCalib(const uint8_t &index) const;
+
+  vector<PANDARLaserCorrection> getCorrection(const uint8_t &index) const;
+
+  vector<vector<bool>> getMask(const uint8_t &index) const;
+
+  Vector3d getCenterToSlamStartOffset();
+
+  void setTrajectoryLoded(bool isloaded);
+
+  bool getTrajectoryLoded() const;
+
+  bool configMeshTaskDescribe();
+
+  bool configPeriodTaskDescribe(const vector<uint32_t> &submapId_vec,
+                                const TrajType traj_type = NODE);
+
+  bool configBagTaskDescribe();
+
+  bool configPcapTaskDescribe(const string &pcapPath = "");
+
+  TaskDescribe getTaskDescribe();
+
+  boost::shared_ptr<Trajectory> getPPKTraj();
+
+  boost::shared_ptr<Trajectory> getNodeTraj();
+
+  vector<pair<string, fpos_t>> getPcapFposVec(uint8_t lidarpos,
+                                              double start_time);
+
+  Vector3d getCenter();
+
+  map<uint8_t, map<string, Time_Duration>> getPcapRangeMap();
+
+  void findPcapsTimeRange(const string &pcapPath = "");
+
+  string getL0SN();
+
+  string getL1SN();
+
+  vector<uint16_t> GetMultiTraj() { return multiTraj_; }
+
+  void configRelativeTask();
+
+ private:
+  void appendSubmapTask(uint32_t submapId, const TrajType traj_type = NODE);
+
+  void loadTrajectory();
+
+  void findPcapFposByDurationStart(const Time_Duration &duration);
+
+  void findPcapTimeRange(const string &pcapfile, uint8_t lidarPosition);
+
+  vector<Time_Duration> segmentBagTimeDurations();
+
+  void LastPacket(const unsigned char *&data, unsigned int dataLength,
+                  double &timeSinceStart, const fpos_t &fpos,
+                  PcapFileReader &reader);
+
+  void findPcapFile(const string &pcapDir, DevicePosition device);
+
+ private:
+  string taskPath_;
+
+  string ppk_dir_;
+
+  string pose_dir_;
+
+  map<uint8_t, Matrix4d> calibMap_;
+
+  Vector3d center_ = Vector3d(0, 0, 0);
+
+  Vector3d center_to_slam_start_offset_ = Vector3d(0, 0, 0);
+
+  map<uint8_t, vector<string>> pcapPathMap_;
+
+  map<uint8_t, vector<string>> bagPathMap_;
+
+  boost::mutex Mtx_;
+
+  bool trajectory_load_ready_ = false;
+
+  map<uint8_t, vector<PANDARLaserCorrection>> correctionMap_;
+
+  TaskDescribe task_describe_;
+
+  boost::shared_ptr<Trajectory> ppk_traj_;
+
+  boost::shared_ptr<Trajectory> node_traj_;
+
+  map<uint8_t, map<string, Time_Duration>> pcapRangeMap_;
+
+  map<uint8_t, map<double, vector<pair<string, fpos_t>>>> durationPcapFposMap_;
+
+  map<uint8_t, LidarType> lidarPositionTypeMap_;
+
+  std::ofstream ofs_;
+
+  map<uint8_t, vector<vector<bool>>> maskMap_;
+
+  uint32_t Device_SN_ = 0;
+
+  string L0_SN_;
+
+  string L1_SN_;
+
+  TaskMode taskMode_;
+
+  vector<uint16_t> multiTraj_;
+
+  map<uint16_t, Isometry3d> taskIndexPoseDiff_;
+
+  map<string, Isometry3d> taskDirPoseDiff_;
+
+  bool meshMerge_ = false;
+
+  string pcdDir_;
+};
+
+#endif  // PARAMSSERVER_H

libs/reader/abstract_reader.cpp

+#include "abstract_reader.h"
+
+AbstractReader::AbstractReader(const string &path,
+                               const vector<Time_Duration> &durations)
+    :path_(path),
+      durations_(durations)
+{
+    sort(durations_.begin(), durations_.end(),
+         [](const Time_Duration& query, const Time_Duration& target) {
+             return query.start_sec < target.start_sec;
+         });
+}
+

libs/register/CMakeLists.txt

+add_library(register STATIC gicp.cpp)
+
+target_link_libraries(register
+    lidar
+    ${PCL_LIBRARIES})

libs/transform/CMakeLists.txt

+add_library(transform STATIC transform.cpp)
+

libs/utils/CMakeLists.txt

+add_library(utils STATIC basic_functions.cpp)
+