add beta pcap2 rosbag converter

Signed-off-by: amc-nu <[email protected]>
MapIV · Mar 22, 2024 · fd7cf23 · fd7cf23
1 parent 7006ddf
commit fd7cf23
Show file tree

Hide file tree

Showing 4 changed files with 233 additions and 0 deletions.
diff --git a/pandar_driver/CMakeLists.txt b/pandar_driver/CMakeLists.txt
@@ -3,9 +3,11 @@ project(pandar_driver)
 
 add_compile_options(-std=c++14)
 
+include(cmake/FindPCAP.cmake)
 
 find_package(catkin REQUIRED COMPONENTS
   roscpp
+  rosbag
   roslib
   nodelet
   pluginlib
@@ -60,6 +62,18 @@ target_link_libraries(pandar_driver_nodelet
   ${catkin_LIBRARIES}
 )
 
+## pcap
+add_executable(pcap2rosbag_converter
+  src/driver/pcap2rosbag_converter.cpp
+)
+target_include_directories(pcap2rosbag_converter PRIVATE
+  ${PCAP_INCLUDE_DIR}
+)
+target_link_libraries(pcap2rosbag_converter
+  ${PCAP_LIBRARY}
+  ${catkin_LIBRARIES}
+)
+
 
 # Install
 ## executables and libraries
@@ -68,6 +82,7 @@ install(
     pandar_driver_node
     pandar_driver_nodelet
     pandar_input
+    pcap2rosbag_converter
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

diff --git a/pandar_driver/cmake/FindPCAP.cmake b/pandar_driver/cmake/FindPCAP.cmake
@@ -0,0 +1,86 @@
+# - 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 Include/pcap.h
+)
+
+find_path(PCAP_INCLUDE_DIR
+    NAMES pcap.h
+    HINTS ${PCAP_ROOT_DIR}/include
+)
+
+if ( MSVC AND COMPILER_ARCHITECTURE STREQUAL "x86_64" )
+    set(_pcap_lib_hint_path ${PCAP_ROOT_DIR}/lib/x64)
+else()
+    set(_pcap_lib_hint_path ${PCAP_ROOT_DIR}/lib)
+endif()
+
+find_library(PCAP_LIBRARY
+    NAMES pcap wpcap
+    HINTS ${_pcap_lib_hint_path}
+)
+
+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 ()
+
+string(FIND "${PCAP_LIBRARY}" "wpcap" _pcap_lib_is_wpcap)
+if ( _pcap_lib_is_wpcap GREATER_EQUAL 0 )
+    set(HAVE_WPCAP TRUE)
+endif()
+
+include(CheckFunctionExists)
+set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY})
+check_function_exists(pcap_get_pfring_id HAVE_PF_RING)
+check_function_exists(pcap_dump_open_append HAVE_PCAP_DUMP_OPEN_APPEND)
+set(CMAKE_REQUIRED_LIBRARIES)
+
+mark_as_advanced(
+    PCAP_ROOT_DIR
+    PCAP_INCLUDE_DIR
+    PCAP_LIBRARY
+)
diff --git a/pandar_driver/package.xml b/pandar_driver/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>roscpp</depend>
+  <depend>rosbag</depend>
   <depend>roslib</depend>
   <depend>nodelet</depend>
   <depend>pluginlib</depend>

diff --git a/pandar_driver/src/driver/pcap2rosbag_converter.cpp b/pandar_driver/src/driver/pcap2rosbag_converter.cpp
@@ -0,0 +1,131 @@
+#include <iostream>
+#include <pcap.h>
+
+#include <pandar_msgs/PandarPacket.h>
+#include <pandar_msgs/PandarScan.h>
+#include <rosbag/bag.h>
+
+class PacketGenerator {
+private:
+    pcap_t* pcap_pointer_;
+    std::string output_bag_path_;
+    size_t data_port_;
+    size_t azimuth_index_;
+    std::function<bool(size_t)> is_valid_packet_;
+    std::string model_;
+public:
+    PacketGenerator(const std::string& pcap_path, const std::string& model, size_t data_port, const std::string& output_bag) {
+        if(pcap_path.empty()) {
+            std::cerr << "pcap file path is empty" << std::endl;
+            exit(1);
+        }
+        if (model.empty()) {
+            std::cerr << "Sensor moodel is empty" << std::endl;
+            exit(1);
+        }
+        if (output_bag.empty()) {
+            std::cerr << "Output bag is empty" << std::endl;
+            exit(1);
+        }
+        output_bag_path_ = output_bag;
+        model_ = model;
+        if (model_ == "Pandar40P" || model_ == "Pandar40M") {
+            azimuth_index_ = 2;  // 2 + 124 * [0-9]
+            is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1262 || packet_size == 1266); };
+        }
+        else if (model_ == "PandarQT") {
+            azimuth_index_ = 12;  // 12 + 258 * [0-3]
+            is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1072); };
+        }
+        else  if (model_ == "PandarXT-32") {
+            azimuth_index_ = 12;  // 12 + 130 * [0-7]
+            is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1080); };
+        }
+        else if (model_ == "Pandar64") {
+            azimuth_index_ = 8;  // 8 + 192 * [0-5]
+            is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1194 || packet_size == 1198); };
+        }
+        else if (model_ == "Pandar128E4X") {
+            azimuth_index_ = 12;  // 12 + 386 * [0-1]
+            is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1117 || packet_size == 861); };
+        }
+        else if (model_ == "PandarXTM") {
+            azimuth_index_ = 12;  // 12 + 130 * [0-7]
+            is_valid_packet_ = [](size_t packet_size) { return (packet_size == 820); };
+        }
+        else if (model_ == "PandarQT128") {
+            azimuth_index_ = 12;  // 12 + 512 * [0-1]
+            is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1127); };
+        }
+        else {
+            ROS_ERROR("Invalid model name");
+        }
+
+        pcap_pointer_ = pcap_open_offline(pcap_path.c_str(), nullptr);
+        if (pcap_pointer_ == nullptr) {
+            std::cerr << "Error opening pcap file:" << pcap_path << std::endl;
+            exit(1);
+        }
+    }
+
+    void process() {
+
+        rosbag::Bag bag(output_bag_path_, rosbag::bagmode::Write);
+        //bag.write("pandar_packets", ros::Time::now(), generator.pcap_pointer_);
+        pandar_msgs::PandarScanPtr pandar_scan_ptr(new pandar_msgs::PandarScan);
+        pandar_scan_ptr->header.frame_id = "pandar";
+        struct pcap_pkthdr pcap_header;
+        const u_char* pcap_packet;
+        const uint8_t PKT_HEADER_SIZE = 42;
+        int current_phase = 0;
+        int prev_phase = 0;
+        while ((pcap_packet = pcap_next(pcap_pointer_, &pcap_header)) != nullptr) {
+            ros::Time ros_time;
+            ros_time.sec = pcap_header.ts.tv_sec;
+            ros_time.nsec = pcap_header.ts.tv_usec * 1000;
+            //if (pcap_header.len >= PKT_HEADER_SIZE) 
+            {
+                auto data_size = pcap_header.len - PKT_HEADER_SIZE;
+                if (is_valid_packet_(data_size)) {
+                    pandar_msgs::PandarPacket pandar_packet;
+                    pandar_packet.stamp = ros_time;
+                    memcpy(&pandar_packet.data, (pcap_packet+PKT_HEADER_SIZE), data_size);
+                    pandar_packet.size = data_size;
+                    current_phase = (pandar_packet.data[azimuth_index_] & 0xff) | ((pandar_packet.data[azimuth_index_ + 1] & 0xff) << 8);
+                    if (current_phase > prev_phase) {
+                        pandar_scan_ptr->packets.push_back(pandar_packet);
+                        prev_phase = current_phase;
+                    }
+                    else {
+                        prev_phase = current_phase;
+                        pandar_scan_ptr->header.stamp = pandar_scan_ptr->packets.front().stamp;
+                        bag.write("pandar_packets", ros_time, pandar_scan_ptr);
+                        pandar_scan_ptr->packets.clear();
+                        pandar_scan_ptr->packets.push_back(pandar_packet);
+                    }
+                }//end valid packet
+            }//end valid length
+        } //end while
+    }//end process
+
+    ~PacketGenerator() {
+        if (pcap_pointer_ != nullptr)
+            pcap_close(pcap_pointer_);
+    }
+};
+
+
+
+
+int main(int argc, char *argv[]) {
+    if (argc < 5) {
+        std::cerr << "Usage: pcap_reader <pcap_file> <sensor_model> <udp_data_port> <output_rosbag>" << std::endl;
+        std::cerr << "sensor_model: Pandar40P, Pandar64, PandarQT, Pandar128E4X, PandarXT-32, PandarXTM, PandarQT128" << std::endl;
+        exit(1);
+    }
+    ros::Time::init();
+    PacketGenerator generator(argv[1], argv[2], atoi(argv[3]), argv[4]);
+    generator.process();
+
+    return 0;
+}