feat: Add lidar_model parameter.

CMakeLists.txt

@@ -4,6 +4,7 @@ project(mapora)
 set(CMAKE_CXX_STANDARD 17)
 
 add_compile_options(-Wall -Wextra -Wpedantic)
+list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake")
 
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
@@ -30,22 +31,22 @@ include_directories(include
 set(MAPORA_LIB_SRC
         src/mapora.cpp
         src/mapora_rosbag.cpp
-        src/points_provider_velodyne_vlp16.cpp
-        src/continuous_packet_parser_vlp16.cpp
-        src/transform_provider_applanix.cpp
+        src/points_provider.cpp
+        src/parsers/velodyne_vlp16.cpp
+        src/parsers/hesai_xt32.cpp
+        src/transform_provider.cpp
         src/utils.cpp)
 
 set(MAPORA_LIB_HEADERS
         include/mapora/mapora.hpp
         include/mapora/mapora_rosbag.hpp
         include/mapora/date.h
         include/mapora/csv.hpp
-        include/mapora/point_xyzi.hpp
-        include/mapora/point_xyzit.hpp
-        include/mapora/points_provider_base.hpp
-        include/mapora/points_provider_velodyne_vlp16.hpp
-        include/mapora/continuous_packet_parser_vlp16.hpp
-        include/mapora/transform_provider_applanix.hpp
+        include/mapora/point_types.hpp
+        include/mapora/points_provider.hpp
+        include/mapora/parsers/velodyne_vlp16.hpp
+        include/mapora/parsers/hesai_xt32.hpp
+        include/mapora/transform_provider.hpp
         include/mapora/utils.hpp
         include/mapora/visibility_control.hpp)
 
@@ -56,9 +57,9 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
         ${MAPORA_LIB_SRC})
 target_link_libraries(${PROJECT_NAME}
         ${Boost_LIBRARIES}
-        ${PcapPlusPlus_LIBRARIES}
+#        ${PcapPlusPlus_LIBRARIES}
         Eigen3::Eigen
-        # PcapPlusPlus::PcapPlusPlus
+         PcapPlusPlus::PcapPlusPlus
         TBB::tbb
         ${GeographicLib_LIBRARIES}
         ${libLAS_LIBRARIES})

cmake/FindPcapPlusPlus.cmake

@@ -0,0 +1,53 @@
+# FindPcapPlusPlus.cmake
+#
+# Finds the PcapPlusPlus library.
+#
+# This will define the following variables
+#
+#    PcapPlusPlus_FOUND
+#    PcapPlusPlus_INCLUDE_DIRS
+#    PcapPlusPlus_LIBRARIES
+#    PcapPlusPlus_VERSION
+#
+# and the following imported targets
+#
+#    PcapPlusPlus::PcapPlusPlus
+#
+
+if (PC_PcapPlusPlus_INCLUDEDIR AND PC_PcapPlusPlus_LIBDIR)
+    set(PcapPlusPlus_FIND_QUIETLY TRUE)
+endif ()
+
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(PC_PcapPlusPlus REQUIRED PcapPlusPlus)
+
+set(PcapPlusPlus_VERSION ${PC_PcapPlusPlus_VERSION})
+
+mark_as_advanced(PcapPlusPlus_INCLUDE_DIR PcapPlusPlus_LIBRARY)
+
+foreach (LIB_NAME ${PC_PcapPlusPlus_LIBRARIES})
+    find_library(${LIB_NAME}_PATH ${LIB_NAME} HINTS ${PC_PcapPlusPlus_LIBDIR})
+    if (${LIB_NAME}_PATH)
+        list(APPEND PcapPlusPlus_LIBS ${${LIB_NAME}_PATH})
+    endif ()
+endforeach ()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(PcapPlusPlus
+        REQUIRED_VARS PC_PcapPlusPlus_INCLUDEDIR PC_PcapPlusPlus_LIBDIR
+        VERSION_VAR PcapPlusPlus_VERSION
+        )
+
+if (PcapPlusPlus_FOUND)
+    set(PcapPlusPlus_INCLUDE_DIRS ${PC_PcapPlusPlus_INCLUDEDIR})
+    set(PcapPlusPlus_LIBRARIES ${PcapPlusPlus_LIBS})
+endif ()
+
+if (PcapPlusPlus_FOUND AND NOT TARGET PcapPlusPlus::PcapPlusPlus)
+    add_library(PcapPlusPlus::PcapPlusPlus INTERFACE IMPORTED)
+    set_target_properties(PcapPlusPlus::PcapPlusPlus PROPERTIES
+            INTERFACE_INCLUDE_DIRECTORIES "${PcapPlusPlus_INCLUDE_DIRS}"
+            INTERFACE_LINK_LIBRARIES "${PcapPlusPlus_LIBRARIES}"
+            INTERFACE_COMPILE_FLAGS "${PC_PcapPlusPlus_CFLAGS}"
+            )
+endif ()

include/mapora/mapora.hpp

@@ -17,13 +17,13 @@
 #ifndef MAPORA__MAPORA_HPP_
 #define MAPORA__MAPORA_HPP_
 
+#include "mapora/points_provider.hpp"
+#include "mapora/transform_provider.hpp"
 #include "mapora/visibility_control.hpp"
-#include "mapora/points_provider_velodyne_vlp16.hpp"
-#include "mapora/transform_provider_applanix.hpp"
-#include <rclcpp/rclcpp.hpp>
 #include <memory>
-#include <sensor_msgs/msg/point_cloud2.hpp>
 #include <nav_msgs/msg/path.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_ros/transform_broadcaster.h>
 
@@ -39,7 +39,8 @@ class Mapora : public rclcpp::Node
   using SharedPtr = std::shared_ptr<Mapora>;
   using ConstSharedPtr = const std::shared_ptr<Mapora>;
   using PointCloud2 = sensor_msgs::msg::PointCloud2;
-  using Points = points_provider::PointsProviderBase::Points;
+  using Point = point_types::PointXYZITRH;
+  using Points = std::vector<Point>;
 
   explicit Mapora(const rclcpp::NodeOptions & options);
 
@@ -55,23 +56,31 @@ class Mapora : public rclcpp::Node
   double max_point_distance_from_lidar_;
   double min_point_distance_from_lidar_;
   std::string las_export_dir;
+  std::string lidar_model;
 
-  std::vector<points_provider::PointsProviderVelodyneVlp16::Points> clouds;
+  std::vector<Points> clouds;
 
 private:
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_ptr_cloud_current_;
-  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_ptr_path_applanix_;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_ptr_path;
 
-  transform_provider_applanix::TransformProviderApplanix::SharedPtr transform_provider_applanix;
-  points_provider::PointsProviderVelodyneVlp16::SharedPtr points_provider_velodyne_vlp16;
+  transform_provider::TransformProvider::SharedPtr
+      transform_provider;
+  points_provider::PointsProvider::SharedPtr points_provider_;
 
   PointCloud2::SharedPtr points_to_cloud(const Points & points_bad, const std::string & frame_id);
 
   void callback_cloud_surround_out(const Points & points_surround);
 
+  Points transform_points(Points & cloud);
+
+
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
-  transform_provider_applanix::TransformProviderApplanix::Pose pose;
+  transform_provider::TransformProvider::Pose pose;
+
+  Point last_point;
 
+  int file_counter = 0;
 };
 }  // namespace mapora
 

include/mapora/parsers/hesai_xt32.hpp

@@ -0,0 +1,170 @@
+//
+// Created by ataparlar on 11.10.2024.
+//
+
+#ifndef MAPORA__CONTINUOUS_PACKET_PARSER_HESAI_XT32_HPP_
+#define MAPORA__CONTINUOUS_PACKET_PARSER_HESAI_XT32_HPP_
+
+
+#include "mapora/date.h"
+#include "mapora/point_types.hpp"
+
+#include <pcapplusplus/Packet.h>
+
+#include <cstdint>
+#include <functional>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace mapora::points_provider::continuous_packet_parser
+{
+class ContinuousPacketParserXt32 {
+public:
+  using SharedPtr = std::shared_ptr<ContinuousPacketParserXt32>;
+  using ConstSharedPtr = const std::shared_ptr<ContinuousPacketParserXt32>;
+  using Point = point_types::PointXYZITRH;
+  using Points = std::vector<Point>;
+
+  ContinuousPacketParserXt32();
+
+  void process_packet_into_cloud(
+      const pcpp::RawPacket & rawPacket,
+      const std::function<void(const Points &)> & callback_cloud_surround_out,
+      const float min_point_distance_from_lidar,
+      const float max_point_distance_from_lidar);
+
+private:
+  using uint8_t = std::uint8_t;
+  using uint16_t = std::uint16_t;
+
+  struct PreHeader
+  {
+    uint8_t start_of_packet_1;
+    uint8_t start_of_packet_2;
+    uint8_t protocol_version_major;
+    uint8_t protocol_version_minor;
+    uint16_t reserved;
+  } __attribute__((packed));
+
+  struct Header
+  {
+    uint8_t laser_number;
+    uint8_t block_number;
+    uint8_t first_block_return;
+    uint8_t dis_unit;
+    uint8_t return_number;
+    uint8_t udp_seq;
+  } __attribute__((packed));
+
+  struct DataPoint
+  {
+    uint16_t distance_divided_by_4mm;
+    uint8_t reflectivity;
+    uint8_t reserved;
+  } __attribute__((packed));
+
+  struct DataBlock  // each 130 byte
+  {
+    uint16_t azimuth_multiplied_by_100_deg;
+    DataPoint data_points[32];
+    [[nodiscard]] size_t get_size_data_points() const
+    {
+      return sizeof(data_points) / sizeof(data_points[0]);
+    }
+  } __attribute__((packed));
+
+  struct Tail
+  {
+    uint8_t reserved[9];
+    uint8_t high_temp_shutdown;
+    uint8_t return_mode;
+    uint16_t motor_speed;
+    uint8_t year;
+    uint8_t month;
+    uint8_t day;
+    uint8_t hour;
+    uint8_t minute;
+    uint8_t second;
+    uint32_t timestamp;
+    uint8_t factory_info;
+  } __attribute__((packed));
+
+  struct DataPacket {
+    uint8_t udp_header[42];
+    PreHeader pre_header;
+    Header header;
+    DataBlock data_blocks[8];
+    Tail tail;
+    uint32_t udp_seq;;
+    size_t get_size_data_blocks() const
+    {
+      return sizeof(data_blocks) / sizeof(data_blocks[0]);
+    }
+  };
+
+
+  enum class ReturnMode { Strongest, LastReturn, DualReturn };
+
+  std::map<uint8_t, ReturnMode> map_byte_to_return_mode_;
+  std::map<ReturnMode, std::string> map_return_mode_to_string_;
+
+  enum class HesaiModel
+  {
+    Pandar128,  // 1
+    ET,         // 2
+    QT,         // 3
+    AT128,      // 4
+    PandarXT,   // 6
+    PandarFT    // 7
+  };
+
+
+  struct PositionPacket
+  {
+    uint8_t udp_header[42];
+    uint16_t header;
+    uint8_t date[6];
+    uint8_t time[6];
+    uint32_t microsecond_time;
+    char nmea_sentence[84];
+    uint8_t reserved[404];
+    uint8_t gps_positioning_status;
+    uint8_t status_pps;
+    uint32_t reserved_2;
+    //    uint8_t temp_when_last_shut_from_overheat;
+    //    uint8_t temp_when_booted;
+    //    char nmea_sentence[128];
+    //    uint8_t reserved_03[178];
+  } __attribute__((packed));
+
+
+  HesaiModel hesai_model_;
+  ReturnMode return_mode_;
+
+  bool factory_bytes_are_read_at_least_once_;
+  bool has_received_valid_position_package_;
+
+  date::sys_time<std::chrono::hours> tp_hours_since_epoch;
+
+  std::map<uint8_t, HesaiModel> map_byte_to_hesai_model_;
+  std::map<HesaiModel, std::string> map_hesai_model_to_string_;
+
+  std::vector<float> channel_to_angle_vertical_;
+  std::vector<float> channel_mod_8_to_azimuth_offsets_;
+  std::vector<size_t> ind_block_to_first_channel_;
+
+  bool has_processed_a_packet_;
+  float angle_deg_azimuth_last_packet_;
+  uint32_t microseconds_last_packet_;
+
+  Points cloud_;
+  bool can_publish_again_;
+  float angle_deg_cut_;
+};
+}
+
+
+
+#endif // MAPORA__CONTINUOUS_PACKET_PARSER_HESAI_XT32_HPP_

include/mapora/continuous_packet_parser_vlp16.hpp → include/mapora/parsers/velodyne_vlp16.hpp

@@ -14,26 +14,28 @@
 * limitations under the License.
  */
 
-#ifndef MAPORA__CONTINUOUS_PACKET_PARSER_HPP_
-#define MAPORA__CONTINUOUS_PACKET_PARSER_HPP_
+#ifndef MAPORA__CONTINUOUS_PACKET_PARSER_VLP16_HPP_
+#define MAPORA__CONTINUOUS_PACKET_PARSER_VLP16_HPP_
 
 #include <map>
 #include <memory>
 #include <vector>
 #include <cstdint>
 #include <string>
 #include <functional>
-#include "point_xyzit.hpp"
+#include "mapora/point_types.hpp"
 #include "mapora/date.h"
 #include <pcapplusplus/Packet.h>
 
 
-namespace mapora::points_provider::continuous_packet_parser_vlp16
+namespace mapora::points_provider::continuous_packet_parser
 {
 class ContinuousPacketParserVlp16
 {
 public:
-  using Point = point_types::PointXYZIT;
+  using SharedPtr = std::shared_ptr<ContinuousPacketParserVlp16>;
+  using ConstSharedPtr = const std::shared_ptr<ContinuousPacketParserVlp16>;
+  using Point = point_types::PointXYZITRH;
   using Points = std::vector<Point>;
 
   ContinuousPacketParserVlp16();
@@ -135,7 +137,7 @@ class ContinuousPacketParserVlp16
   bool can_publish_again_;
   float angle_deg_cut_;
 };
-}  // namespace mapora::point_provider::continuous_packet_parser_vlp16
+}  // namespace mapora::point_provider::continuous_packet_parser
 
 
-#endif  // MAPORA__CONTINUOUS_PACKET_PARSER_HPP_
+#endif  // MAPORA__CONTINUOUS_PACKET_PARSER_VLP16_HPP_