Merge branch 'feature/v3_parsing_and_metadata' into 'main'

Feature/v3 parsing and metadata

Closes #243

See merge request syntron/support/csr/ifm3d/ifm3d!344

ChangeLog.md

@@ -5,6 +5,11 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
 and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
 
 ## [Unreleased]
+### Added
+- Parsing of V3 chunks
+- Ability to access V3 chunk metadata (see Frame::Metadata)
+- Ability to access multiple chunks with the same id in a frame (see Frame::GetBuffer and Frame::GetBufferCount)
+- O3R_RESULT_JSON and O3R_RESULT_ARRAY2D chunk ids
 
 ### Changes
 - Use .deb files in Dockerfile instead of building from sources

modules/device/include/ifm3d/device/device.h

@@ -114,6 +114,8 @@ namespace ifm3d
     ALGO_DEBUG = 900,
     O3R_ODS_OCCUPANCY_GRID = 1000,
     O3R_ODS_INFO = 1001,
+    O3R_RESULT_JSON = 1002,
+    O3R_RESULT_ARRAY2D = 1003
   };
 
   class XMLRPCWrapper;

modules/device/include/ifm3d/device/err.h

@@ -64,6 +64,7 @@ extern IFM3D_DEVICE_EXPORT const int IFM3D_CORRUPTED_STRUCT;
 extern IFM3D_DEVICE_EXPORT const int
   IFM3D_DEVICE_PORT_INCOMPATIBLE_WITH_ORGANIZER;
 extern IFM3D_DEVICE_EXPORT const int IFM3D_DEVICE_PORT_NOT_SUPPORTED;
+extern IFM3D_DEVICE_EXPORT const int IFM3D_INDEX_OUT_OF_RANGE;
 // sensor errors
 extern IFM3D_DEVICE_EXPORT const int IFM3D_XMLRPC_OBJ_NOT_FOUND;
 extern IFM3D_DEVICE_EXPORT const int IFM3D_INVALID_PARAM;

modules/device/src/libifm3d_device/err.cpp

@@ -48,6 +48,7 @@ const int IFM3D_SYSTEM_ERROR = -100034;
 const int IFM3D_CORRUPTED_STRUCT = -100035;
 const int IFM3D_DEVICE_PORT_INCOMPATIBLE_WITH_ORGANIZER = -100036;
 const int IFM3D_DEVICE_PORT_NOT_SUPPORTED = -100037;
+const int IFM3D_INDEX_OUT_OF_RANGE = -100038;
 // sensor errors
 const int IFM3D_XMLRPC_OBJ_NOT_FOUND = 100000;
 const int IFM3D_INVALID_PARAM = 101000;
@@ -162,6 +163,8 @@ ifm3d::strerror(int errnum)
              "organizers";
     case IFM3D_DEVICE_PORT_NOT_SUPPORTED:
       return "Lib: Port is not supported by the device";
+    case IFM3D_INDEX_OUT_OF_RANGE:
+      return "Lib:  index is out of range";
     case IFM3D_XMLRPC_OBJ_NOT_FOUND:
       return "Sensor: XMLRPC obj not found - trying to access dead session?";
     case IFM3D_INVALID_PARAM:

modules/framegrabber/include/ifm3d/fg/buffer.h

@@ -10,6 +10,8 @@
 #include <cstdint>
 #include <memory>
 #include <vector>
+#include <optional>
+#include <ifm3d/common/json.hpp>
 #include <limits.h>
 #include <ifm3d/fg/frame_grabber_export.h>
 #include <ifm3d/device/device.h>
@@ -114,6 +116,8 @@ namespace ifm3d
     size_t bytes_per_pixel;
     /* @brief bytes per row */
     size_t bytes_per_row;
+    /* @brief json formatted metadata of the chunk obtain from device*/
+    json metadata_;
 
     class BufferAllocator;
     std::shared_ptr<BufferAllocator> buffer_allocator_;
@@ -132,13 +136,15 @@ namespace ifm3d
       @param nchannel Number of channels in Buffer
       @param format value from ifm3d::pixel_format releates to data type
       need to store one value.
+      @param metadata json formatted metadata of the chunk obtain from device
 
       @note This internally calls Create Method to allocates Memory
     */
     Buffer(const std::uint32_t cols,
            const std::uint32_t rows,
            const std::uint32_t nchannel,
-           ifm3d::pixel_format format);
+           ifm3d::pixel_format format,
+           std::optional<ifm3d::json> metadata = std::nullopt);
 
     virtual ~Buffer() = default;
 
@@ -172,6 +178,7 @@ namespace ifm3d
     std::uint32_t width() const;
     std::uint32_t nchannels() const;
     ifm3d::pixel_format dataFormat() const;
+    ifm3d::json metadata() const;
 
     /**
      * @brief Return the size of the buffer in bytes
@@ -321,7 +328,9 @@ namespace ifm3d
 
     /* Similar to Buffer(cols,rows,ifm3d::formatType<Tp>::nchannel,
      * ifm3d::formatType<Tp>::format ) */
-    Buffer_(const std::uint32_t cols, const std::uint32_t rows);
+    Buffer_(const std::uint32_t cols,
+            const std::uint32_t rows,
+            std::optional<ifm3d::json> metadata = std::nullopt);
 
     ~Buffer_() = default;
 
@@ -349,6 +358,7 @@ namespace ifm3d
     std::uint32_t width() const;
     std::uint32_t nchannels() const;
     ifm3d::pixel_format dataFormat() const;
+    ifm3d::json metadata() const;
 
     /** @brief returns a pointer to the specified Buffer row.
         @param row number

modules/framegrabber/include/ifm3d/fg/detail/buffer.hpp

@@ -314,12 +314,15 @@ template <typename Tp>
 ifm3d::Buffer_<Tp>::Buffer_() : ifm3d::Buffer(){};
 
 template <typename Tp>
-ifm3d::Buffer_<Tp>::Buffer_(const std::uint32_t cols, const std::uint32_t rows)
+ifm3d::Buffer_<Tp>::Buffer_(const std::uint32_t cols,
+                            const std::uint32_t rows,
+                            std::optional<ifm3d::json> metadata)
   : ifm3d::Buffer(
       cols,
       rows,
       static_cast<std::uint32_t>(ifm3d::FormatType<Tp>::nchannel),
-      static_cast<ifm3d::pixel_format>(ifm3d::FormatType<Tp>::format))
+      static_cast<ifm3d::pixel_format>(ifm3d::FormatType<Tp>::format),
+      metadata)
 {}
 
 template <typename Tp>
@@ -390,6 +393,13 @@ ifm3d::Buffer_<Tp>::dataFormat() const
   return Buffer::dataFormat();
 }
 
+template <typename Tp>
+ifm3d::json
+ifm3d::Buffer_<Tp>::metadata() const
+{
+  return Buffer::metadata();
+}
+
 template <typename Tp>
 Tp*
 ifm3d::Buffer_<Tp>::ptr(const std::uint32_t row)

modules/framegrabber/include/ifm3d/fg/frame.h

@@ -149,35 +149,55 @@ namespace ifm3d
     */
     ALGO_DEBUG = static_cast<uint64_t>(ifm3d::image_chunk::ALGO_DEBUG), 
 
-    /** @hideinitializer 
+    /** @hideinitializer
      * @brief ifm3d::ODSOccupancyGridV1
     */
     O3R_ODS_OCCUPANCY_GRID = static_cast<uint64_t>(ifm3d::image_chunk::O3R_ODS_OCCUPANCY_GRID), 
 
-    /** @hideinitializer 
+    /** @hideinitializer
      * @brief ifm3d::ODSInfoV1
     */
     O3R_ODS_INFO = static_cast<uint64_t>(ifm3d::image_chunk::O3R_ODS_INFO), 
 
+    /** @hideinitializer
+     * @brief ifm3d::O3R_RESULT_JSON
+    */
+    O3R_RESULT_JSON = static_cast<uint64_t>(ifm3d::image_chunk::O3R_RESULT_JSON),
+
+    /** @hideinitializer 
+     * @brief ifm3d::O3R_RESULT_ARRAY2D
+    */
+    O3R_RESULT_ARRAY2D = static_cast<uint64_t>(ifm3d::image_chunk::O3R_RESULT_ARRAY2D),
+
+
     /** @hideinitializer 
      * @brief The point cloud encoded as a 3 channel XYZ image
     */
-    XYZ = std::numeric_limits<std::uint32_t>::max(),  
+    XYZ = std::numeric_limits<std::uint32_t>::max(),
 
     /** @hideinitializer 
      * @brief \c
      */
-    EXPOSURE_TIME,  
+    EXPOSURE_TIME,
 
     /** @hideinitializer 
      * @brief \c
     */
     ILLUMINATION_TEMP,
+
+    O3R_ODS_FLAGS,
+    O3R_MCC_LIVE_IMAGE,
+    O3R_MCC_MOTION_IMAGE,
+    O3R_MCC_STATIC_IMAGE,
+
     // clang-format on
   };
   using TimePointT = std::chrono::time_point<std::chrono::system_clock,
                                              std::chrono::nanoseconds>;
 
+  using BufferList = std::vector<Buffer>;
+  using BufferDataListMap = std::map<ifm3d::buffer_id, BufferList>;
+
   /** @ingroup FrameGrabber
    *
    * Represent a frame of data received from the the device.
@@ -187,7 +207,7 @@ namespace ifm3d
   public:
     using Ptr = std::shared_ptr<Frame>;
 
-    Frame(const std::map<buffer_id, Buffer>& images,
+    Frame(const BufferDataListMap& images,
           const std::vector<TimePointT> timestamps,
           uint64_t frame_count);
     ~Frame();
@@ -218,10 +238,17 @@ namespace ifm3d
      * @brief Get the image with the given id
      *
      * @param id the id of the image to get
+     * @param index of the image
      * @return Image& Reference to the requested buffer
      * @throw std::out_of_range if no image with the give id exists
      */
-    Buffer& GetBuffer(buffer_id id);
+    Buffer& GetBuffer(buffer_id id,
+                      std::optional<size_t> index = std::nullopt);
+
+    /**
+     * @brief Get the total number of image with the given id
+     */
+    size_t GetBufferCount(buffer_id id);
 
     /**
      * @brief Get the frame count according to algorithm output
@@ -236,12 +263,13 @@ namespace ifm3d
      */
     std::vector<buffer_id> GetBuffers();
 
-    decltype(std::declval<std::map<buffer_id, Buffer>>().begin())
+    decltype(std::declval<std::map<buffer_id, BufferList>>().begin())
     begin() noexcept;
-    decltype(std::declval<const std::map<buffer_id, Buffer>>().begin()) begin()
-      const noexcept;
-    decltype(std::declval<std::map<buffer_id, Buffer>>().end()) end() noexcept;
-    decltype(std::declval<const std::map<buffer_id, Buffer>>().end()) end()
+    decltype(std::declval<const std::map<buffer_id, BufferList>>().begin())
+    begin() const noexcept;
+    decltype(std::declval<std::map<buffer_id, BufferList>>().end())
+    end() noexcept;
+    decltype(std::declval<const std::map<buffer_id, BufferList>>().end()) end()
       const noexcept;
 
   private:

modules/framegrabber/include/ifm3d/fg/organizer.h

@@ -24,7 +24,7 @@ namespace ifm3d
   public:
     struct Result
     {
-      std::map<buffer_id, Buffer> images;
+      std::map<buffer_id, BufferList> images;
       std::vector<ifm3d::TimePointT> timestamps;
       uint32_t frame_count;
     };

modules/framegrabber/include/ifm3d/fg/organizer_utils.h

@@ -7,6 +7,7 @@
 #define IFM3D_FG_ORGANIZER_UTILS_H
 
 #include <map>
+#include <set>
 #include <optional>
 #include <vector>
 #include <tuple>
@@ -20,7 +21,7 @@ namespace ifm3d
 {
   constexpr std::size_t IMG_BUFF_START = 8;
 
-  std::map<image_chunk, std::size_t> get_image_chunks(
+  std::map<image_chunk, std::set<std::size_t>> get_image_chunks(
     const std::vector<std::uint8_t>& data,
     std::size_t start_idx,
     std::optional<size_t> end_idx = std::nullopt);
@@ -40,7 +41,8 @@ namespace ifm3d
                        std::size_t idx,
                        std::size_t width,
                        std::size_t height,
-                       pixel_format fmt);
+                       pixel_format fmt,
+                       std::optional<json> metadata = std::nullopt);
 
   Buffer create_xyz_buffer(const std::vector<std::uint8_t>& data,
                            std::size_t xidx,
@@ -51,7 +53,8 @@ namespace ifm3d
                            pixel_format fmt,
                            const std::optional<Buffer>& mask);
 
-  auto find_metadata_chunk(const std::map<image_chunk, std::size_t>& chunks)
+  auto find_metadata_chunk(
+    const std::map<image_chunk, std::set<std::size_t>>& chunks)
     -> decltype(chunks.end());
 
   std::tuple<uint32_t, uint32_t> get_image_size(
@@ -77,6 +80,9 @@ namespace ifm3d
   std::size_t get_chunk_pixeldata_size(const std::vector<std::uint8_t>& data,
                                        std::size_t idx);
 
+  std::size_t get_chunk_header_version(const std::vector<std::uint8_t>& data,
+                                       std::size_t idx);
+
   void mask_buffer(Buffer& image, const Buffer& mask);
 
   bool is_probably_blob(const std::vector<std::uint8_t>& data,
@@ -86,18 +92,24 @@ namespace ifm3d
 
   Buffer create_pixel_mask(Buffer& confidence);
 
-  void parse_data(const std::vector<uint8_t>& data,
-                  const std::set<buffer_id>& requestedImages,
-                  const std::map<ifm3d::image_chunk, std::size_t>& chunks,
-                  const size_t width,
-                  const size_t height,
-                  std::map<buffer_id, Buffer>& data_blob,
-                  std::map<buffer_id, Buffer>& data_image);
-
-  void mask_images(std::map<ifm3d::buffer_id, ifm3d::Buffer>& images,
+  void parse_data(
+    const std::vector<uint8_t>& data,
+    const std::set<buffer_id>& requestedImages,
+    const std::map<ifm3d::image_chunk, std::set<std::size_t>>& chunks,
+    const size_t width,
+    const size_t height,
+    std::map<buffer_id, BufferList>& data_blob,
+    std::map<buffer_id, BufferList>& data_image);
+
+  void mask_images(std::map<ifm3d::buffer_id, ifm3d::BufferList>& images,
                    ifm3d::Buffer& mask,
                    std::function<bool(ifm3d::buffer_id id)> should_mask);
 
+  bool has_metadata(const std::vector<std::uint8_t>& data, std::size_t idx);
+
+  ifm3d::json create_metadata(const std::vector<std::uint8_t>& data,
+                              std::size_t idx);
+
   /**
    * Create a value of type T from sizeof(T) bytes of the passed in byte
    * buffer. Given that the ifm sensors transmit data in little endian

modules/framegrabber/src/libifm3d_framegrabber/buffer.cpp

@@ -90,13 +90,16 @@ ifm3d::Buffer::Buffer()
     data_size_in_bytes_(0),
     size_(0),
     bytes_per_pixel(0),
-    bytes_per_row(0)
+    bytes_per_row(0),
+    metadata_(ifm3d::json())
 {}
 
 ifm3d::Buffer::Buffer(const std::uint32_t cols,
                       const std::uint32_t rows,
                       const std::uint32_t nchannel,
-                      ifm3d::pixel_format format)
+                      ifm3d::pixel_format format,
+                      std::optional<ifm3d::json> metadata)
+  : metadata_(metadata.value_or(ifm3d::json()))
 {
   create(cols, rows, nchannel, format);
 }
@@ -129,8 +132,7 @@ ifm3d::Buffer::create(const std::uint32_t cols,
 ifm3d::Buffer
 ifm3d::Buffer::clone() const
 {
-  Buffer copy;
-  copy.create(cols_, rows_, nchannel_, data_format_);
+  Buffer copy(cols_, rows_, nchannel_, data_format_, metadata_);
   std::memcpy(copy.ptr(0), data_, size_);
   return copy;
 }
@@ -163,4 +165,10 @@ size_t
 ifm3d::Buffer::size() const
 {
   return size_;
+}
+
+ifm3d::json
+ifm3d::Buffer::metadata() const
+{
+  return metadata_;
 }