Merge pull request #37 from nocoinman/feature-pcap

[FEATURE] Handling and publishing from PCAP files

Author: tibordome

CMakeLists.txt

@@ -24,6 +24,13 @@ find_package(catkin REQUIRED COMPONENTS
 ## System dependencies are found with CMake's conventions
 find_package(Boost REQUIRED COMPONENTS system thread regex)
 
+## For PCAP file handling
+find_library(libpcap_LIBRARIES pcap)
+if ("${libpcap_LIBRARIES}" STREQUAL "pcap-NOTFOUND")
+    set(libpcap_FOUND FALSE)
+else ()
+    set(libpcap_FOUND TRUE)
+endif ()
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
@@ -150,7 +157,21 @@ include_directories(
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(${PROJECT_NAME}_node src/septentrio_gnss_driver/node/rosaic_node.cpp src/septentrio_gnss_driver/communication/circular_buffer.cpp src/septentrio_gnss_driver/parsers/parsing_utilities.cpp src/septentrio_gnss_driver/parsers/string_utilities.cpp src/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.cpp src/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.cpp src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.cpp src/septentrio_gnss_driver/crc/crc.c src/septentrio_gnss_driver/communication/communication_core.cpp src/septentrio_gnss_driver/communication/rx_message.cpp src/septentrio_gnss_driver/communication/callback_handlers.cpp)
+add_executable(${PROJECT_NAME}_node 
+    src/septentrio_gnss_driver/node/rosaic_node.cpp 
+    src/septentrio_gnss_driver/communication/circular_buffer.cpp 
+    src/septentrio_gnss_driver/parsers/parsing_utilities.cpp 
+    src/septentrio_gnss_driver/parsers/string_utilities.cpp 
+    src/septentrio_gnss_driver/parsers/nmea_parsers/gpgga.cpp 
+    src/septentrio_gnss_driver/parsers/nmea_parsers/gprmc.cpp 
+    src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.cpp 
+    src/septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.cpp 
+    src/septentrio_gnss_driver/crc/crc.c 
+    src/septentrio_gnss_driver/communication/communication_core.cpp 
+    src/septentrio_gnss_driver/communication/rx_message.cpp 
+    src/septentrio_gnss_driver/communication/callback_handlers.cpp
+    src/septentrio_gnss_driver/communication/pcap_reader.cpp
+)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -164,7 +185,7 @@ add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catk
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}_node 
-   ${catkin_LIBRARIES} ${Boost_LIBRARIES}
+   ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${libpcap_LIBRARIES}
 )
 
 #############

README.md

@@ -17,6 +17,8 @@ Please [let the maintainers know](mailto:[email protected]?subject=[GitH
 The `master` branch for this driver functions on both ROS Melodic (Ubuntu 18.04) and Noetic (Ubuntu 20.04). It is thus necessary to [install](https://wiki.ros.org/Installation/Ubuntu) the ROS version that has been designed for your Linux distro.<br><br>
 The serial and TCP/IP communication interface of the ROS driver is established by means of the [Boost C++ library](https://www.boost.org/). In the unlikely event that the below installation instructions fail to install Boost on the fly, please install the Boost libraries via<br><br>
 `sudo apt install libboost-all-dev`.<br><br>
+Compatiblity with PCAP captures are incorporated through [pcap libraries](https://github.com/the-tcpdump-group/libpcap). Install the necessary headers via<br><br>
+`sudo apt install libpcap-dev`.<br><br>
 
 ## Usage
 The binary release is now available for Melodic, yet will take another few days for Noetic. To install the binary package on Melodic, simply run<br><br>
@@ -116,6 +118,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
   - `device`: location of device connection
     - `serial:xxx` format for serial connections, where `xxx` is the device node, e.g. `serial:/dev/ttyUSB0`
     - `file_name:path/to/file.sbf` format for publishing from an SBF log
+    - `file_name:path/to/file.pcap` format for publishing from PCAP capture.
       - Regarding the file path, ROS_HOME=\`pwd\` in front of `roslaunch septentrio...` might be useful to specify that the node should be started using the executable's directory as its working-directory.
     - `tcp://host:port` format for TCP/IP connections
       - `28784` should be used as the default (command) port for TCP/IP connections. If another port is specified, the receiver needs to be (re-)configured via the Web Interface before ROSaic can be used.
@@ -206,7 +209,6 @@ A selection of NMEA sentences, the majority being standardized sentences, and pr
 
 ## Suggestions for Improvements
 - Automatic Search: If the host address of the receiver is omitted in the `host:port` specification, the driver could automatically search and establish a connection on the specified port.
-- Incorporating PCAP: For PCAP connections the ROS parameter `device` could be generalized to accept the path to the .pcap file. In this case, the node could exit automatically after finishing playback.
 - Publishing the topic `/measepoch`: It could accept the custom ROS message `septentrio_gnss_driver/MeasEpoch.msg`, corresponding to the SBF block `MeasEpoch` (raw GNSS data).
 - Publishing the topic `/twist`: It could accept the generic ROS message [`geometry_msgs/TwistWithCovarianceStamped.msg`](https://docs.ros.org/melodic/api/geometry_msgs/html/msg/TwistWithCovarianceStamped.html), converted from the SBF blocks `PVTGeodetic`, `PosCovGeodetic` and others or via standardized NMEA sentences (cf. the [NMEA driver](https://wiki.ros.org/nmea_navsat_driver)).
   - The ROS message [`geometry_msgs/TwistWithCovarianceStamped.msg`](https://docs.ros.org/melodic/api/geometry_msgs/html/msg/TwistWithCovarianceStamped.html) could be fed directly into the [`robot_localization`](https://docs.ros.org/melodic/api/robot_localization/html/index.html) nodes of the ROS navigation stack.

include/septentrio_gnss_driver/communication/communication_core.hpp

@@ -142,6 +142,14 @@ namespace io_comm_rx {
          * @param[in] file_name The name of (or path to) the SBF file, e.g. "xyz.sbf"
          */
         void initializeSBFFileReading(std::string file_name);
+
+        /**
+         * @brief Initializes PCAP file reading and reads PCAP file by repeatedly
+         * calling read_callback_()
+         * @param[in] file_name The name of (or path to) the PCAP file, e.g. "/tmp/capture.pcap"
+         */
+        void initializePCAPFileReading(std::string file_name);
+
         /**
          * @brief Set the I/O manager
          * @param[in] manager An I/O handler

include/septentrio_gnss_driver/communication/pcap_reader.hpp

@@ -0,0 +1,118 @@
+// *****************************************************************************
+//
+// © Copyright 2020, Septentrio NV/SA.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//    1. Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//    2. Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//    3. Neither the name of the copyright holder nor the names of its
+//       contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// *****************************************************************************
+
+#ifndef PCAP_READER_H
+#define PCAP_READER_H
+
+#include <pcap/pcap.h>
+#include <stdint.h>
+#include <vector>
+
+/**
+ * @file pcap_reader.hpp
+ * @date 07/05/2021
+ * @author Ashwin A Nayar
+ *
+ * @brief Declares a class for handling pcap files.
+ */
+
+namespace pcapReader {
+
+    using buffer_t = std::vector<uint8_t>;
+
+    //! Read operation status
+    enum ReadResult
+    {
+        /// Data read successfully
+        READ_SUCCESS = 0,
+        READ_INSUFFICIENT_DATA = 1,
+        READ_TIMEOUT = 2,
+        READ_INTERRUPTED = 3,
+        READ_ERROR = -1,
+        /// Unable to parse data, parsing error
+        READ_PARSE_FAILED = -2
+
+    };
+
+    /**
+     * @class PcapDevice
+     * @brief Class for handling a pcap file
+     */
+    class PcapDevice
+    {
+    public:
+        static const size_t BUFFSIZE = 100;
+
+        /**
+         * @brief Constructor for PcapDevice
+         * @param[out] buffer Buffer to write read raw data to
+         */
+        explicit PcapDevice(buffer_t& buffer);
+
+        /**
+         * @brief Try to open a pcap file
+         * @param[in] device Path to pcap file
+         * @return True if success, false otherwise
+         */
+        bool connect(const char* device);
+
+        /**
+         * @brief Close connected file
+         */
+        void disconnect();
+
+        /**
+         * @brief Check if file is open and healthy
+         * @return True if file is open, false otherwise
+         */
+        bool isConnected() const;
+
+        /**
+         * @brief Attempt to read a packet and store data to buffer
+         * @return Result of read operation
+         */
+        ReadResult read();
+
+        //! Destructor for PcapDevice
+        ~PcapDevice();
+
+    private:
+        //! Reference to raw data buffer to write to
+        buffer_t& m_dataBuff;
+        //! File handle to pcap file
+        pcap_t* m_device{nullptr};
+        bpf_program m_pktFilter{};
+        char m_errBuff[BUFFSIZE]{};
+        char* m_deviceName;
+        buffer_t m_lastPkt;
+    };
+} // namespace pcapReader
+
+#endif // PCAP_READER_H

include/septentrio_gnss_driver/communication/rx_message.hpp

@@ -172,6 +172,7 @@ extern boost::shared_ptr<ros::NodeHandle> g_nh;
 extern const uint32_t g_ROS_QUEUE_SIZE;
 extern ros::Time g_unix_time;
 extern bool g_read_from_sbf_log;
+extern bool g_read_from_pcap;
 
 //! Enum for NavSatFix's status.status field, which is obtained from PVTGeodetic's
 //! Mode field

include/septentrio_gnss_driver/node/rosaic_node.hpp

@@ -219,6 +219,12 @@ namespace rosaic_node {
          */
         void prepareSBFFileReading(std::string file_name);
 
+        /**
+         * @brief Sets up the stage for PCAP file reading
+         * @param[in] file_name The path to PCAP file, e.g. "/tmp/capture.sbf"
+         */
+        void preparePCAPFileReading(std::string file_name);
+
         /**
          * @brief Attempts to (re)connect every reconnect_delay_s_ seconds
          */

package.xml

@@ -55,6 +55,7 @@
   <depend>geometry_msgs</depend>
   <depend>gps_common</depend>
   <depend>boost</depend>
+  <depend>libpcap</depend>
 
 
   <build_depend>cpp_common</build_depend>

src/septentrio_gnss_driver/communication/communication_core.cpp

@@ -29,6 +29,7 @@
 // *****************************************************************************
 
 #include <septentrio_gnss_driver/communication/communication_core.hpp>
+#include <septentrio_gnss_driver/communication/pcap_reader.hpp>
 
 /**
  * @file communication_core.cpp
@@ -159,6 +160,58 @@ void io_comm_rx::Comm_IO::initializeSBFFileReading(std::string file_name)
     ROS_DEBUG("Leaving initializeSBFFileReading() method..");
 }
 
+void io_comm_rx::Comm_IO::initializePCAPFileReading(std::string file_name)
+{
+    ROS_DEBUG("Calling initializePCAPFileReading() method..");
+    pcapReader::buffer_t vec_buf;
+    pcapReader::PcapDevice device(vec_buf);
+
+    if (!device.connect(file_name.c_str()))
+    {
+        ROS_ERROR("Unable to find file or either it is corrupted");
+        return;
+    }
+
+    ROS_INFO("Reading ...");
+    while (device.isConnected() && device.read() == pcapReader::READ_SUCCESS)
+        ;
+    device.disconnect();
+
+    std::size_t buffer_size = pcapReader::PcapDevice::BUFFSIZE;
+    uint8_t* to_be_parsed = new uint8_t[buffer_size];
+    to_be_parsed = &vec_buf[0];
+
+    while (1) // Loop will stop if we are done reading the SBF file
+    {
+        try
+        {
+            ROS_DEBUG(
+                "Calling read_callback_() method, with number of bytes to be parsed being %li",
+                buffer_size);
+            handlers_.readCallback(to_be_parsed, buffer_size);
+        } catch (std::size_t& parsing_failed_here)
+        {
+            if (to_be_parsed - &vec_buf[0] >= vec_buf.size() * sizeof(uint8_t))
+            {
+                break;
+            }
+            if (!parsing_failed_here)
+                parsing_failed_here = 1;
+
+            to_be_parsed = to_be_parsed + parsing_failed_here;
+            ROS_DEBUG("Parsing_failed_here is %li", parsing_failed_here);
+            continue;
+        }
+        if (to_be_parsed - &vec_buf[0] >= vec_buf.size() * sizeof(uint8_t))
+        {
+            break;
+        }
+        to_be_parsed = to_be_parsed + buffer_size;
+    }
+    delete[] to_be_parsed;
+    ROS_DEBUG("Leaving initializePCAPFileReading() method..");
+}
+
 bool io_comm_rx::Comm_IO::initializeSerial(std::string port, uint32_t baudrate,
                                            std::string flowcontrol)
 {