How to stream ODS data
By default, this example uses the configuration available with the examples.
ods_get_data.cpp
/*
* Copyright 2021-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_config.hpp"
#include "ods_get_data.hpp"
#include <ifm3d/deserialize/struct_o3r_ods_info_v1.hpp>
#include <ifm3d/deserialize/struct_o3r_ods_occupancy_grid_v1.hpp>
#include <ifm3d/device/o3r.h>
int main() {
///////////////////////////////////////////////////
// Variables needed for the example
///////////////////////////////////////////////////
std::string config_extrinsic_path = "../configs/extrinsic_one_head.json";
std::string config_app_path = "../configs/ods_one_head_config.json";
// Declare the device object (one object only, corresponding to the VPU)
auto o3r = std::make_shared<ifm3d::O3R>();
////////////////////////////////////////////////
// Reset the configuration so that we configure
// exactly what this example expects.
////////////////////////////////////////////////
std::clog << "Resetting the applications" << std::endl;
o3r->Reset({"/applications"});
try {
std::clog << "Trying to get data from app before instantiating"
<< std::endl;
ODSStream ods_stream(o3r, "app0",
{ifm3d::buffer_id::O3R_ODS_INFO,
ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID},
500, 5);
} catch (...) { // Failing silently to continue with the tutorial.
std::clog << "ODSStream cannot be configured with inexistent app0.\n"
<< "This is expected, continuing with the example." << std::endl;
}
ODSConfig ods_config(o3r);
// TODO: change path to config file
// Assuming a camera facing forward, label up,
// 60 cm above the floor.
// We keep the extrinsic calibration and the ODS configuration
// separate for clarity. You can keep all configurations
// in one file if necessary.
ods_config.SetConfigFromFile(config_extrinsic_path);
ods_config.SetConfigFromFile(config_app_path);
// We did not start the application when configuring it,
// so we need to start it now (change state to "RUN")
ods_config.SetConfigFromStr(
R"({"applications": {"instances": {"app0": {"state": "RUN"}}}})");
////////////////////////////////////////////////
// Instantiate the ODSStream object: we expect an
// app called "app0". If your app is different,
// change the app name below.
////////////////////////////////////////////////
std::string app_name = "app0"; // HERE change to your app name
// Here we are starting data streams for both the
// zones and the occupancy grid. This can be changed
// by removing unwanted data streams from the buffer
// list.
ODSStream ods_stream(o3r, app_name,
{ifm3d::buffer_id::O3R_ODS_INFO,
ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID},
500, 5);
ods_stream.StartODSStream();
std::this_thread::sleep_for(std::chrono::seconds(2));
// Loop and display current zones and occupancy grid information
// for duration d
int d = 5;
for (auto start = std::chrono::steady_clock::now(), now = start;
now < start + std::chrono::seconds{d};
now = std::chrono::steady_clock::now()) {
auto zones = ods_stream.GetZones();
auto grid = ods_stream.GetOccGrid();
if (zones){
std::clog << "Current zone occupancy:\n"
<< 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;
}
if (grid){
std::clog << "Current occupancy grid's middle cell:\n"
<< std::to_string(grid.value().image.at<uint8_t>(100, 100)) << std::endl;
}
}
std::cout << "Finished getting data from ODS\n";
ods_stream.StopODSStream();
return 0;
}
ods_get_data.h
/*
* Copyright 2022-present ifm electronic, gmbh
* SPDX-License-Identifier: Apache-2.0
*/
#include <chrono>
#include <ifm3d/deserialize/struct_o3r_ods_info_v1.hpp>
#include <ifm3d/deserialize/struct_o3r_ods_occupancy_grid_v1.hpp>
#include <ifm3d/device/o3r.h>
#include <ifm3d/fg.h>
#include <iostream>
#include <queue>
#include <stdexcept>
#include <thread>
using namespace ifm3d::literals;
class ODSDataQueue {
public:
std::queue<ifm3d::Buffer> zones_queue;
std::queue<ifm3d::Buffer> occ_grid_queue;
int queue_size;
ODSDataQueue(int queue_size_ = 5): queue_size(queue_size_){}
void AddFrame(ifm3d::Frame::Ptr frame) {
if (frame->HasBuffer(ifm3d::buffer_id::O3R_ODS_INFO)) {
if (zones_queue.size() > queue_size) {
zones_queue.pop();
}
zones_queue.push(frame->GetBuffer(ifm3d::buffer_id::O3R_ODS_INFO));
}
if (frame->HasBuffer(ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID)) {
if (occ_grid_queue.size() > queue_size) {
zones_queue.pop();
}
occ_grid_queue.push(
frame->GetBuffer(ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID));
}
}
ifm3d::ODSInfoV1 GetZones() {
// Calling this function on an empty queue will cause undefined behavior.
auto zones = ifm3d::ODSInfoV1::Deserialize(zones_queue.front());
zones_queue.pop();
return zones;
}
ifm3d::ODSOccupancyGridV1 GetOccGrid() {
// Calling this function on an empty queue will cause undefined behavior.
auto grid = ifm3d::ODSOccupancyGridV1::Deserialize(occ_grid_queue.front());
occ_grid_queue.pop();
return grid;
}
};
class ODSStream {
public:
ODSStream(ifm3d::O3R::Ptr o3r_, std::string app_name_,
ifm3d::FrameGrabber::BufferList buffer_ids_, int timeout_, int queue_size_): o3r(o3r_), app_name(app_name_), buffer_ids(buffer_ids_), timeout(timeout_), queue_size(queue_size_), data_queue(queue_size){
std::string j_string =
"/applications/instances/" + app_name + "/data/pcicTCPPort";
ifm3d::json::json_pointer j(j_string);
auto FG_PCIC_PORT = o3r->Get({j_string})[j];
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) { this->data_queue.AddFrame(frame); });
}
void StopODSStream() {
std::clog << "Stopping data stream" << std::endl;
fg->Stop();
}
std::optional<ifm3d::ODSInfoV1> GetZones() {
if (!data_queue.zones_queue.empty()) {
auto zones = data_queue.GetZones();
return zones;
}
return {};
}
std::optional<ifm3d::ODSOccupancyGridV1> GetOccGrid() {
if (!data_queue.occ_grid_queue.empty()) {
auto occ_grid = data_queue.GetOccGrid();
return occ_grid;
}
return {};
}
private:
ifm3d::O3R::Ptr o3r;
std::string app_name;
ifm3d::FrameGrabber::Ptr fg;
ifm3d::FrameGrabber::BufferList buffer_ids = {
ifm3d::buffer_id::O3R_ODS_INFO, ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID};
int queue_size = 5;
ODSDataQueue data_queue;
int timeout = 500; // Timeout in milliseconds
};