How to stream ODS data

By default, this example uses the configuration available with the examples.

ods_get_data.cpp
/*
 * Copyright 2025-present ifm electronic, gmbh
 * SPDX-License-Identifier: Apache-2.0
 *
 * This example assumes that an instance of ODS is configured
 * and switched to RUN state.
 */
#include <iostream>
#include <stdexcept>
#include <thread>

#include "ods_get_data.hpp"

#include <ifm3d/common/json.hpp>
#include <ifm3d/deserialize/struct_o3r_ods_extrinsic_calibration_correction_v1.hpp>
#include <ifm3d/deserialize/struct_o3r_ods_info_v1.hpp>
#include <ifm3d/deserialize/struct_o3r_ods_occupancy_grid_v1.hpp>
#include <ifm3d/deserialize/struct_o3r_ods_polar_occupancy_grid_v1.hpp>
#include <ifm3d/device/o3r.h>

void async_diagnostic_callback(const std::string &message,
                               const std::string &app_name) {
  using ifm3d::json;

  // Parse the diagnostic message
  json diagnostic = json::parse(message);

  // Extract groups and check the status of the specified application
  auto groups = diagnostic.value("groups", json::object());
  std::string app_status = groups.value(app_name, "unknown");

  std::cout << "\nNew Diagnostic: The status of application '" << app_name
            << "': " << app_status << std::endl;

  // Check if the application is in a critical state
  if (app_status == "critical") {
    std::cout << "⚠️ Application '" << app_name
              << "' is in a critical state! Stop the Robot!!" << std::endl;
  }
}

int main() {
  // Device specific configuration
  std::string IP = "192.168.0.69";
  auto o3r = std::make_shared<ifm3d::O3R>(IP);
  std::string app_name = "app0"; // Change to your app name

  ODSStream ods_stream(
      o3r, app_name,
      {ifm3d::buffer_id::O3R_ODS_INFO, ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID,
       ifm3d::buffer_id::O3R_ODS_POLAR_OCC_GRID,
       ifm3d::buffer_id::O3R_ODS_EXTRINSIC_CALIBRATION_CORRECTION},
      500, 5);
  ods_stream.StartODSStream();
  std::this_thread::sleep_for(std::chrono::seconds(2));

  auto diag_fg = std::make_shared<ifm3d::FrameGrabber>(o3r, 50009);
  diag_fg->OnAsyncError([app_name](int id, const std::string &message) {
    async_diagnostic_callback(message, app_name);
  });
  std::clog << "Starting async diagnostic monitoring. \nErrors ids and "
               "descriptions will be logged."
            << std::endl;
  diag_fg->Start({});

  while (true) {
    auto zones = ods_stream.GetZones();
    auto grid = ods_stream.GetOccGrid();
    auto polar_grid = ods_stream.GetPolarOccGrid();
    auto ods_extrinsic_calibration_correction =
        ods_stream.GetExtrinsicCalibrationCorrection();

    if (zones) {
      std::clog << "-------------ODS zones data --------------------------"
                << std::endl;
      std::clog << "Current zone id used: " << zones.value().zone_config_id
                << std::endl;
      std::clog << "Zones occupancy: "
                << std::to_string(zones.value().zone_occupied[0]) << ", "
                << std::to_string(zones.value().zone_occupied[1]) << ", "
                << std::to_string(zones.value().zone_occupied[2]) << std::endl;
      std::clog << "Zones info timestamp: " << zones.value().timestamp_ns
                << std::endl;
    }

    if (grid) {
      std::clog << "--------------ODS occupancy grid data------------------"
                << std::endl;
      std::clog << "Occupancy grid image shape: " << grid.value().image.height()
                << "x" << grid.value().image.width() << std::endl;
      std::clog << "Occupancy grid timestamp: " << grid.value().timestamp_ns
                << std::endl;
    }

    if (polar_grid) {
      std::clog << "--------------ODS polar occupancy grid data ----------"
                << std::endl;
      auto distance_0degree =
          polar_grid.value().polarOccGrid[0] /
          1000.0; // distances are in mm, the 360° are divided into 675 values
      if (distance_0degree ==
          65.535) { // 65.535 is a special value for no object detected
        std::clog << "No object detected at 0° using the Polar occupancy grid"
                  << std::endl;
      } else {
        std::clog << "Distance to the first object at 0° using the Polar "
                     "occupancy grid: "
                  << distance_0degree << " m" << std::endl;
      }
    }

    if (ods_extrinsic_calibration_correction) {
      const auto &calibration_data =
          ods_extrinsic_calibration_correction.value();
      std::clog << "-------------Extrinsic Calibration Correction data "
                   "--------------------------"
                << std::endl;
      std::clog << "rot_delta_valid [x,y,z] : "
                << std::to_string(calibration_data.rot_delta_valid[0]) << ", "
                << std::to_string(calibration_data.rot_delta_valid[1]) << ", "
                << std::to_string(calibration_data.rot_delta_valid[2])
                << std::endl;
      std::clog << "rot_head_to_user [x,y,z] : "
                << std::to_string(calibration_data.rot_head_to_user[0]) << ", "
                << std::to_string(calibration_data.rot_head_to_user[1]) << ", "
                << std::to_string(calibration_data.rot_head_to_user[2])
                << std::endl;
    } else {
      std::clog << "No valid extrinsic calibration correction data available."
                << std::endl;
    }
    std::this_thread::sleep_for(std::chrono::seconds(1));
  }

  std::cout << "Finished getting data from ODS\n";
  ods_stream.StopODSStream();
  diag_fg->Stop();

  return 0;
}
ods_get_data.h
/*
 * Copyright 2025-present ifm electronic, gmbh
 * SPDX-License-Identifier: Apache-2.0
 */
#include <chrono>
#include <ifm3d/deserialize/struct_o3r_ods_extrinsic_calibration_correction_v1.hpp>
#include <ifm3d/deserialize/struct_o3r_ods_info_v1.hpp>
#include <ifm3d/deserialize/struct_o3r_ods_occupancy_grid_v1.hpp>
#include <ifm3d/deserialize/struct_o3r_ods_polar_occupancy_grid_v1.hpp>
#include <ifm3d/device/o3r.h>
#include <ifm3d/fg.h>
#include <iostream>
#include <optional>
#include <queue>
#include <stdexcept>
#include <thread>

using namespace ifm3d::literals;

class ODSStream {
public:
  ODSStream(ifm3d::O3R::Ptr o3r_, const std::string &app_name_,
            ifm3d::FrameGrabber::BufferList buffer_ids_, int timeout_,
            int queue_size_ = 5)
      : o3r(o3r_), app_name(app_name_), buffer_ids(buffer_ids_),
        timeout(timeout_), queue_size(queue_size_) {
    auto FG_PCIC_PORT = o3r->Port(app_name).pcic_port;
    fg = std::make_shared<ifm3d::FrameGrabber>(o3r, FG_PCIC_PORT);
  }

  void StartODSStream() {
    std::clog << "Starting data stream" << std::endl;
    fg->Start(buffer_ids);
    fg->OnNewFrame([this](auto frame) { AddFrame(frame); });
  }

  void StopODSStream() {
    std::clog << "Stopping data stream" << std::endl;
    fg->Stop();
  }

  std::optional<ifm3d::ODSInfoV1> GetZones() {
    return GetFromQueue<ifm3d::ODSInfoV1>(zones_queue, timeout);
  }
  std::optional<ifm3d::ODSOccupancyGridV1> GetOccGrid() {
    return GetFromQueue<ifm3d::ODSOccupancyGridV1>(occ_grid_queue, timeout);
  }
  std::optional<ifm3d::ODSPolarOccupancyGridV1> GetPolarOccGrid() {
    return GetFromQueue<ifm3d::ODSPolarOccupancyGridV1>(polar_occ_grid_queue,
                                                        timeout);
  }
  std::optional<ifm3d::ODSExtrinsicCalibrationCorrectionV1>
  GetExtrinsicCalibrationCorrection() {
    return GetFromQueue<ifm3d::ODSExtrinsicCalibrationCorrectionV1>(
        extrinsic_calibration_correction_queue, timeout);
  }

private:
  template <typename T>
  std::optional<T> GetFromQueue(std::queue<ifm3d::Buffer> &queue,
                                int timeout_ms) {
    auto start = std::chrono::steady_clock::now();

    while (std::chrono::duration_cast<std::chrono::milliseconds>(
               std::chrono::steady_clock::now() - start)
               .count() <= timeout_ms) {
      if (!queue.empty()) {
        auto data = T::Deserialize(queue.front());
        queue.pop();
        return data;
      }
      std::this_thread::sleep_for(
          std::chrono::milliseconds(1)); // Prevent CPU overuse
    }
    std::clog << "Timeout waiting for data" << std::endl; // Log the timeout
    return std::nullopt; // Return nullopt instead of throwing an exception
  }

  void AddFrame(ifm3d::Frame::Ptr frame) {
    AddToQueue(frame, ifm3d::buffer_id::O3R_ODS_INFO, zones_queue);
    AddToQueue(frame, ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID, occ_grid_queue);
    AddToQueue(frame, ifm3d::buffer_id::O3R_ODS_POLAR_OCC_GRID,
               polar_occ_grid_queue);
    AddToQueue(frame,
               ifm3d::buffer_id::O3R_ODS_EXTRINSIC_CALIBRATION_CORRECTION,
               extrinsic_calibration_correction_queue);
  }

  void AddToQueue(ifm3d::Frame::Ptr frame, ifm3d::buffer_id id,
                  std::queue<ifm3d::Buffer> &queue) {
    if (frame->HasBuffer(id)) {
      if (queue.size() > queue_size)
        queue.pop();
      queue.push(frame->GetBuffer(id));
    }
  }

  ifm3d::O3R::Ptr o3r;
  std::string app_name;
  ifm3d::FrameGrabber::Ptr fg;
  ifm3d::FrameGrabber::BufferList buffer_ids;
  int timeout;
  int queue_size;

  std::queue<ifm3d::Buffer> zones_queue;
  std::queue<ifm3d::Buffer> occ_grid_queue;
  std::queue<ifm3d::Buffer> polar_occ_grid_queue;
  std::queue<ifm3d::Buffer> extrinsic_calibration_correction_queue;
};