sensorFusionQuery.h

/********************************************************************************
 * Copyright (c) 2020 HLRS, University of Stuttgart
 *               2018-2020 in-tech GmbH
 *               2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
 *               2022-2025 Hexad GmbH
 *
 * This program and the accompanying materials are made available under the
 * terms of the Eclipse Public License 2.0 which is available at
 * http://www.eclipse.org/legal/epl-2.0.
 *
 * SPDX-License-Identifier: EPL-2.0
 ********************************************************************************/

//-----------------------------------------------------------------------------
//! @file  SensorFusionQuery.h
//! @brief This file provides methods to retrieve structured basic information
//!        from sensorData provided by SensorFusionImplementation::Output().
//-----------------------------------------------------------------------------

#pragma once

#include <map>
#include <vector>

#include "osi3/osi_sensordata.pb.h"

namespace SensorFusionHelperFunctions
{
    static std::vector<osi3::DetectedMovingObject> RetrieveMovingObjectsBySensorId(const std::vector<int> &sensorIds, const osi3::SensorData &sensorData)
    {
        std::vector<osi3::DetectedMovingObject> result;
        auto detectedMovingObjects = sensorData.moving_object();
        for (const auto &object : detectedMovingObjects)
        {
            if (sensorIds.empty())
            {
                result.push_back(object);
                continue;
            }
            for (auto sensorId : object.header().sensor_id())
            {
                if (std::count(sensorIds.cbegin(), sensorIds.cend(), sensorId.value()) > 0)
                {
                    result.push_back(object);
                    break;
                }
            }
        }
        return result;
    }

    static std::vector<osi3::DetectedStationaryObject> RetrieveStationaryObjectsBySensorId(const std::vector<int> &sensorIds, const osi3::SensorData &sensorData)
    {
        std::vector<osi3::DetectedStationaryObject> result;
        auto detectedStationaryObjects = sensorData.stationary_object();
        for (const auto &object : detectedStationaryObjects)
        {
            if (sensorIds.empty())
            {
                result.push_back(object);
                continue;
            }
            for (auto sensorId : object.header().sensor_id())
            {
                if (std::count(sensorIds.cbegin(), sensorIds.cend(), sensorId.value()) > 0)
                {
                    result.push_back(object);
                    break;
                }
            }