Parsing of data from the M3 Sonar API to ROS2 PointCloud2 for usage i…

…n autonomous systems. (#7)

* started imb format parser

* Packet header parser works

* fixed some bugs. Created some more

* Fixed some bugs eirik made. probably added some more

* Removed tcp header from TEST_REAL_DATA and tested

* chatGPT added tcp client and ring buffer

* random

* added ouster ringbuffer

* made simpler read and write functions

* worked on tcpserver, started on new test

* commented out tests that caused compilation fail

* fix

* added wireshark data

* added wireshark data

* added data files

* new client listener

* created c++ script that connects to m3 api

* restucture + more bugs

* parsing packageheader and dataheader working

* parsing complete

* creating m3 publisher

* communication listener publisher working

* pcl creation but not population

* Fixed listner from sonar publishing data to ros2 pcl

* parameters working. last update before doing some stupid shit

* sonar working + cleaning

* added parameter file

* Update README.md

* fixed review changes + polish

---------

Co-authored-by: Eirik Kolås <kolaseirik@gmail.com>

.gitignore

@@ -0,0 +1,7 @@
+.vscode
+
+build
+install
+devel
+log
+lcov

CMakeLists.txt

@@ -12,17 +12,26 @@ endif()
 
 # Find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(PCL REQUIRED)
 
 # Specify the include directories
 include_directories(include)
 
-# Create executable
-add_executable(${PROJECT_NAME}_main
-    src/main.cpp
-    src/api/m3CommandGenerator.cpp
-    src/api/m3ResponseParser.cpp
+add_executable(M3Publisher
+  src/imb/ImbFormat.cpp
+  src/imb/M3Listener.cpp
+  src/imb/M3Publisher.cpp
 )
-ament_target_dependencies(${PROJECT_NAME}_main
+
+ament_target_dependencies(M3Publisher
+    pcl_conversions
+    Eigen3
+    rclcpp
+    sensor_msgs
 )
 
 # Install the headers
@@ -31,28 +40,42 @@ install(
     DESTINATION include
 )
 
+install(
+    DIRECTORY test/data
+    DESTINATION data
+)
+
 # Install the library
 install(TARGETS 
-    ${PROJECT_NAME}_main
+    M3Publisher
     DESTINATION lib/${PROJECT_NAME}
 )
+target_link_libraries(M3Publisher
+  pcl_common
+  ${PCL_LIBRARIES}
+)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   ament_add_gtest(${PROJECT_NAME}_test 
-    test/main.cpp
-    test/XmlCommandGeneratorTest.cpp
-    test/XmlResponseParserTest.cpp
-
-    src/api/m3CommandGenerator.cpp
-    src/api/m3ResponseParser.cpp
+    test/ImbFormatTest.cpp
+    src/imb/ImbFormat.cpp
+
   )
   target_include_directories(${PROJECT_NAME}_test PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
   )
   ament_target_dependencies(${PROJECT_NAME}_test
+    pcl_conversions
+    Eigen3
+    rclcpp
+    sensor_msgs
   )
+  target_link_libraries(${PROJECT_NAME}_test
+    pcl_common
+    ${PCL_LIBRARIES}
+)
 
 endif()
 

README.md

@@ -1,2 +1,10 @@
 # vortex-m3-sonar-driver
 A driver for the Kongsberg M3 Sonar
+
+## Specifications
+Requires an onboard CPU running Ubunto 22.04 and a CPU running Windows for the M3 API.
+
+1. Start the M3 API on Windows, and connect to the sonar head.
+2. Edit the parameter file to match the Windows API's ip address and port (defaults to 20001 / 21001). Also choose what ROS2 topic to publish the pointcloud to. The callback_time is given in milliseconds, and decides how often the software will try to get a new image from the API.
+3. Run the M3Publisher file on Ubuntu 22.04 using ROS2.
+

include/imb/ImbFormat.hpp

@@ -0,0 +1,239 @@
+#pragma once
+#include <cstdint>
+#include <cstring>
+#include <iostream>
+#include <arpa/inet.h>
+#include <eigen3/Eigen/Dense>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+
+namespace m3 {
+namespace imb {
+
+// Available data types for the IMB Beamformed Data Format
+enum class DataType : uint16_t {
+    FloatComplex = 0x1002,
+    IntegerMagnitude = 0x2003
+};
+
+#pragma pack(push, 1) // Disable padding for the following structures
+
+// Structure for MUM_TVG_PARAMS
+struct MUM_TVG_PARAMS {
+    uint16_t factorA;   // Factor A, the spreading coefficient
+    uint16_t factorB;   // Factor B, the absorption coefficient in dB/km
+    float factorC;     // Factor C, the TVG curve offset in dB
+    float factorL;     // Factor L, the maximum gain limit in dB};
+};
+
+// Structure for OFFSETS
+struct OFFSETS 
+{
+    float xOffset;    // Translational offset in X axis
+    float yOffset;    // Translational offset in Y axis
+    float zOffset;    // Translational offset in Z axis
+    float xRotOffset; // Rotational offset about X axis in degrees (Pitch offset)
+    float yRotOffset; // Rotational offset about Y axis in degrees (Roll offset)
+    float zRotOffset; // Rotational offset about Z axis in degrees (Yaw offset)
+    uint32_t dwMounting; // Mounting orientation
+};
+
+// Structure for ROTATOR_OFFSETS
+struct ROTATOR_OFFSETS 
+{
+    float fOffsetA;   // Rotator offset A in meters
+    float fOffsetB;   // Rotator offset B in meters
+    float fOffsetR;   // Rotator offset R in meters
+    float fAngle;     // Rotator angle in degrees
+};
+
+// Structure for REAL_TIME_SOUND_SPEED
+struct REAL_TIME_SOUND_SPEED 
+{
+    float fRaw;       // Raw sound speed in m/s directly from sensor readings
+    float fFiltered;  // Filtered sound speed in m/s after a median filter on the raw values
+    float fApplied;   // Applied sound speed in m/s after filtering and thresholding
+    float fThreshold; // User predefined threshold in m/s used to get the applied value
+};
+
+// Structure for PROFILING_DEPTH_TRACKING
+struct PROFILING_DEPTH_TRACKING 
+{
+    int8_t nRangeDeepLimitPercentage;       // Percentage of the maximum range for auto depth algorithm to switch to the next higher range
+    int8_t nRangeShallowLimitPercentage;    // Percentage of the maximum range for auto depth algorithm to switch to the next lower range
+    int8_t nMinPingRateInHz;                // Minimum ping rate in Hz required by the system
+    int8_t nReserved[5];                    // Reserved field to keep the structure aligned
+};
+
+// Structure for GPS_QUALITY_PARAS
+struct GPS_QUALITY_PARAS 
+{
+    int16_t wQualIndicator;         // GPS quality indicator
+    int16_t wNSat;                  // Number of Satellites in use
+    float fHorizDilution;           // Horizontal dilution of precision
+    int16_t wPosFixQualityCm;       // Position fixed quality in cm
+    int16_t wReserved;              // Reserved field to keep the structure aligned
+};
+
+// Structure for Packet Header
+struct PacketHeader 
+{
+    uint16_t synchronizationWord[4];    // Synchronization word, always 0x8000
+    DataType dataType;                  // Data type
+    uint16_t reservedField;             // Reserved field
+    uint32_t reservedBytes[10];         // 40 reserved bytes
+    uint32_t packet_body_size;            // Packet body size
+
+    /// @brief Default constructor for PacketHeader. Read the raw bytes from memory
+    /// @param byte_array Start address in the memory
+    PacketHeader(const uint8_t* byte_array);
+};
+
+// Structure for Data Header
+struct DataHeader 
+{
+    uint32_t dwBeamformed_Data_Version;                 // Version of this header
+    uint32_t dwSonarID;                                 // Sonar identification
+    uint32_t dwSonarInfo[8];                            // Sonar information such as serial number, power up configurations
+    uint32_t dwTimeSec;                                 // Time stamp of current ping in seconds elapsed since 1970
+    uint32_t dwTimeMillisec;                            // Milliseconds part of current ping
+    float fSoundSpeed;                                  // Speed of sound in m/s for image processing
+    uint32_t nNumImageSample;                           // Total number of image samples
+    float fNearRangeMeter;                              // Minimum mode range in meters
+    float fFarRangeMeter;                               // Maximum mode range in meters
+    float fSWST;                                        // Sampling Window Start Time in seconds
+    float fSWL;                                         // Sampling Window Length in seconds
+    uint16_t nNumBeams;                                 // Total number of beams
+    uint16_t wProcessingType;                           // Processing type (e.g., standard, EIQ, Profiling, etc.)
+    float fBeamList[1024];                              // List of angles for all beams
+    float fImageSampleInterval;                         // Image sample interval in seconds
+    uint16_t nImageDestination;                         // Image designation, 0: main image window, n: zoom image window n, n<=4
+    uint8_t bTXPulseType;                               // Tx pulse type (e.g., CW, FM up sweep, FM down sweep)
+    uint8_t bAzimuthProcessing;                         // Azimuth processing (0: off, 1: on)
+    uint32_t dwModeID;                                  // Unique mode ID, application ID
+    int32_t nNumHybridPRIs;                             // Number of PRIs in a hybrid mode
+    int32_t nHybridIndex;                               // Hybrid mode index
+    uint16_t nPhaseSeqLength;                           // Spatial phase sequence length
+    uint16_t nPhaseSeqIndex;                            // Spatial phase sequence length index
+    uint16_t nNumImages;                                // Number of sub-images for one PRI
+    uint16_t nSubImageIndex;                            // Sub-image index
+    uint32_t dwFrequency;                               // Sonar frequency in Hz
+    uint32_t dwPulseLength;                             // Transmit pulse length in microseconds
+    uint32_t dwPingNumber;                              // Ping counter
+    float fRxFilterBW;                                  // RX filter bandwidth in Hz
+    float fRxNominalResolution;                         // RX nominal resolution in meters
+    float fPulseRepFreq;                                // Pulse Repetition Frequency in Hz
+    char strAppName[128];                               // Application name
+    char strTXPulseName[64];                            // TX pulse name
+    MUM_TVG_PARAMS sTVGParameters;                      // TVG Parameters
+    float fCompassHeading;                              // Heading of current ping in decimal degrees
+    float fMagneticVariation;                           // Magnetic variation in decimal degrees
+    float fPitch;                                       // Pitch in decimal degrees
+    float fRoll;                                        // Roll in decimal degrees
+    float fDepth;                                       // Depth in meters
+    float fTemperature;                                 // Temperature in degrees Celsius
+    OFFSETS sOffsets;                                   // Deployment configuration including translational and rotational offsets
+    double dbLatitude;                                  // Latitude of current ping in decimal degrees
+    double dbLongitude;                                 // Longitude of current ping in decimal degrees
+    float fTXWST;                                       // TX Window Start Time in seconds
+    uint8_t bHeadSensorVersion;                         // Head Sensor Version
+    uint8_t HeadHWStatus;                               // Head hardware status
+    uint8_t bSoundSpeedSource;                          // Source of the sound speed
+    uint8_t bTimeSyncMode;                              // Timer synchronization mode
+    float fInternalSensorHeading;                       // Heading from internal sensor in decimal degrees
+    float fInternalSensorPitch;                         // Pitch from internal sensor in decimal degrees
+    float fInternalSensorRoll;                          // Roll from internal sensor in decimal degrees
+    ROTATOR_OFFSETS sAxesRotatorOffsets[3];             // Rotator offsets
+    uint16_t nStartElement;                             // Start element of the sub array
+    uint16_t nEndElement;                               // End element of the sub array
+    char strCustomText[32];                             // Custom text field
+    char strROVText[32];                                // Custom text field 2
+    float fLocalTimeOffset;                             // Local time zone offset in decimal hours
+    float fVesselSOG;                                   // Vessel Speed Over Ground in knots
+    float fHeave;                                       // Heave in meters
+    float fPRIMinRange;                                 // Minimum PRI range in meters
+    float fPRIMaxRange;                                 // Maximum PRI range in meters
+    float fAltitude;                                    // Altitude in decimal meters
+    float fHeightGeoidAbvEllipsoid;                     // Ellipsoid height in meters
+    REAL_TIME_SOUND_SPEED sRealTimeSoundSpeed;          // Real-time sound speed
+    PROFILING_DEPTH_TRACKING sProfilingDepthTracking;   // Profiling depth tracking
+    GPS_QUALITY_PARAS sGPSQualityParas;                 // GPS quality parameters
+    uint32_t b3DScan;                                   // 3D Scan with rotator
+    uint32_t b3DScanAxis;                               // 3D Scan rotation axis
+    uint8_t bSonarClassID;                              // Sonar type used to collect the data
+    uint8_t byTX_Enabled;                               // For Forward Looking Sonar multiple transducers
+    uint8_t bReserved[2];                               // Reserved field
+    float fROV_Altitude;                                // ROV Altitude
+    float fConductivity;                                // Conductivity
+    uint8_t reserved[3796];                             // reserved field
+
+    /// @brief Default constructor for DataHeader. Read the raw bytes from memory
+    /// @param byte_array Start address in the memory
+    DataHeader(const uint8_t* byte_array);
+};
+
+
+// Structure for Data Body
+struct DataBody 
+{
+    DataType dataType;
+    size_t size;
+    Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic> magnitudeData; // For 8-bit integer magnitude data
+    Eigen::Matrix<std::complex<float>, Eigen::Dynamic, Eigen::Dynamic> complexData; // For complex data
+
+    /**
+     * @brief Construct a new Data Body object from the raw bytes of the TCP packet
+     * 
+     * @param byte_array Raw bytes of the TCP packet
+     * @param nNumBeams Number of beams
+     * @param nNumSamples Number of samples in each beam
+     * @param type Data type (FloatComplex or IntegerMagnitude)
+     * @throw std::runtime_error if the data type is not supported
+     */
+    DataBody(const uint8_t* byte_array, uint16_t nNumBeams, uint16_t nNumSamples, DataType type);
+};
+
+// Structure for Packet Footer
+struct PacketFooter 
+{
+    uint32_t packet_body_size;      // Packet body size (4 bytes)
+    uint32_t reserved_bytes[10];   // 40 reserved bytes (10 x 4 bytes)
+
+    /// @brief Default constructor for PacketFooter
+    /// @param byte_array Address of the first byte of the packet footer
+    PacketFooter(const uint8_t* byte_array);
+};
+
+#pragma pack(pop) // Stop disabling padding
+
+
+
+// Define the combined struct of the IMB Beamformed Data Format
+struct ImbPacketStructure 
+{
+    PacketHeader packet_header;
+    DataHeader data_header;
+    DataBody dataBody;
+    PacketFooter packet_footer;
+
+    mutable bool validated = true;
+    mutable sensor_msgs::msg::PointCloud2 message;
+
+
+    /// @brief Creates the complete IMB packet structure from the raw bytes of the TCP packet collection
+    /// @param byte_array Start of the packet
+    ImbPacketStructure(const uint8_t* byte_array);
+
+    /// @brief Validates the contents of the packet
+    void Validate() const;
+
+    /// @brief Generates a pointcloud from the data
+    void GeneratePointCloud() const;
+
+};
+
+
+
+
+} // namespace imb
+} // namespace m3

include/imb/M3Listener.hpp

@@ -0,0 +1,48 @@
+#pragma once
+#include <imb/ImbFormat.hpp>
+#include <sstream>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <unistd.h>
+#include <mutex>
+#include <exception>
+#include <vector>
+#include <chrono>
+#include <thread>
+
+namespace m3{
+class M3Listener {
+public:
+    const std::string addr_;
+    const uint16_t port_;
+    std::vector<uint8_t>& shared_vector_;
+    bool& packet_ready_;
+    std::mutex& mutex_;
+    uint8_t buffer_[1024 * 64]; // Shared data buffer from the publisher
+    sockaddr_in server_addr_;
+    int client_socket_;
+
+    /// @brief Default constructor
+    /// @param addr Address to the M3 API
+    /// @param port Port to the M3 API (default 20001 / 21001)
+    /// @param shared_vector Vector to write the data to
+    /// @param mutex Shared lock
+    /// @param new_packet Boolean to indicate if a new packet is ready
+    M3Listener(std::string addr, uint16_t port, std::vector<uint8_t>& shared_vector, std::mutex& mutex, bool& new_header);
+
+    /// @brief Creates the socket with the given address and port
+    void create_socket();
+
+    /// @brief Initiates the connection to the M3 API
+    void connect_to_sonar();
+
+    /// @brief Starts listening for data from the M3 API
+    void run_listener();
+
+    /// @brief Closes the socket
+    void stop_listener();
+
+    /// @brief Destructor closing the socket
+    ~M3Listener();
+};
+}