ApproximateTimeQueue.cpp

Go to the documentation of this file.

#include "ApproximateTimeQueue.h"
 
#include <algorithm>
#include <cstdint>
#include <mutex>
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
 
#include "armarx/localization_and_mapping/cartographer_adapter/interfaces.h"
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
 
    ApproximateTimeQueue::ApproximateTimeQueue(const int64_t dt,
                                               const int64_t dtHistoryLength,
                                               const std::set<std::string>& laserSensorIds,
                                               MessageCallee& messageCallee,
                                               bool useOdometry) :
        dtEps(std::max<int64_t>(dt, 0)),
        dtHistoryLength(dtHistoryLength),
        messageCallee(messageCallee),
        useOdometry(useOdometry)
    {
        if (dt > 0)
        {
            ARMARX_ERROR
                << "Requested to throttle processing of data. This is currently not available.";
        }
 
        const std::size_t laserMaxQueueSize = 10;
 
        ARMARX_TRACE;
        for (const auto& laserSensorId : laserSensorIds)
        {
            ARMARX_INFO << VAROUT(laserSensorId);
            laserQueues.insert({laserSensorId, LaserTimeQueue(laserMaxQueueSize)});
        }
        messageCalleeTask =
            new SimpleRunningTask<>([this]() { serveMessageCallee(); }, "Serve Message Callee");
        messageCalleeTask->start();
    }
 
    void
    ApproximateTimeQueue::insertLaserData(LaserScannerMessage message)
    {
        ARMARX_TRACE;
        // ARMARX_INFO << "Laser data " << message->frame << " ts: " << message->timestamp;
 
        // ARMARX_CHECK(laserQueues.count(message.frame) > 0);
        auto entry = laserQueues.find(message.frame);
        if (entry == laserQueues.end())
        {
            ARMARX_INFO << "Received laser data for unknown frame `" << message.frame << "`.";
            return;
        }
        LaserTimeQueue& queue = entry->second;
 
        try
        {
            queue.insert(std::move(message));
            processAllAvailableData(message.timestamp);
        }
        catch (const armarx::LocalException& e)
        {
            ARMARX_INFO << "Unable to insert message into queue. Reason: " << e.what();
            queue.clear();
        }
    }
 
    void
    ApproximateTimeQueue::insertOdomData(
        armarx::localization_and_mapping::cartographer_adapter::PoseStamped odomPose)
    {
        ARMARX_TRACE;
        // ARMARX_INFO << "odom data ts:" << odomPose->timestamp;
        const auto timestamp = odomPose.timestamp;
 
        odomQueue.insert(std::move(odomPose));
        processAllAvailableData(timestamp);
    }
 
    bool
    ApproximateTimeQueue::allDataAvailable(const int64_t timestamp) const
    {
        ARMARX_TRACE;
        auto hasTimestamp = [timestamp](const auto& q) { return q.has(timestamp); };
 
        if (useOdometry and not hasTimestamp(odomQueue))
        {
            return false;
        }
 
        // TODO(fabian.reister): all_of(..., hasTimestamp)
        for (const auto& [sensorName, sensorQueue] : laserQueues)
        {
            if (not hasTimestamp(sensorQueue))
            {
                return false;
            }
        }
 
        return true;
    }
 
    void
    ApproximateTimeQueue::processAllAvailableData(const int64_t timestamp)
    {
        ARMARX_TRACE;
 
        // This is inspired by ROS' approximate time synchronizer, but a bit simpler.
        // It works as follows:
        // (1) Try to find a pivot element. That is an element that has matches in other queues.
        //     The pivot's timestamp is larger than the timestamp of the matches.
        // (2) Start at the beginning of the queues, traverse them and check for matches
        // (3) Once a match is found, it is processed.
        // (4) All messages older than the pivot element are deleted.
 
        std::set<int64_t> timestamps;
 
        const auto insertTimestamps = [&timestamps](std::vector<int64_t> ts)
        {
            if (ts.empty())
            {
                return;
            }
 
            std::copy(ts.begin(), ts.end(), std::inserter(timestamps, timestamps.end()));
        };
 
 
        for (const auto& [_, q] : laserQueues)
        {
            insertTimestamps(q.timestamps());
        }
 
        // std::set is sorted :)
 
        // this will now go through the queues. whenever a match is found, it is processed
        // and older messages are dropped.
        std::for_each(timestamps.begin(),
                      timestamps.end(),
                      [&](const auto& ts) { processAvailableData(ts); });
    }
 
    void
    ApproximateTimeQueue::processAvailableData(const int64_t timestamp)
    {
        ARMARX_TRACE;
        ARMARX_DEBUG << "Processing available data at " << timestamp;
 
        const int64_t dt = timestamp - lastProcessedTimestamp;
 
        if (dt < 0)
        {
            ARMARX_DEBUG << "Trying to process a timestamp that is " << dt
                         << " older than last processed timestamp.";
            return;
        }
 
        // only process data at a certain frequency
        // WARNING: this feature is broken.
        // if (dt <= dtEps)
        // {
        //     return;
        // }
 
        // process no messages that are too old
        // trimUntil(timestamp - dtHistoryLength);
 
        if (not allDataAvailable(timestamp))
        {
            return;
        }
 
        ARMARX_DEBUG << "Processing approximate time data " << timestamp;
 
        std::map<std::string, std::size_t> queueSizes;
        queueSizes["odometry"] = odomQueue.size();
 
        for (const auto& [sensorName, sensorQueue] : laserQueues)
        {
            queueSizes["laser_" + sensorName] = sensorQueue.size();
        }
 
        ARMARX_VERBOSE << deactivateSpam(1) << "queue sizes: " << queueSizes;
 
        lastProcessedTimestamp = timestamp;
 
        try
        {
            ARMARX_TRACE;
 
            const Eigen::Isometry3f odom_T_lookup =
                useOdometry ? odomQueue.lookupInterpolate(timestamp).pose
                            : Eigen::Isometry3f::Identity();
 
            TimedData timedData;
            timedData.timestamp = timestamp;
 
            timedData.odomPose.timestamp = timestamp;
            timedData.odomPose.pose = odom_T_lookup;
 
            for (const auto& [sensorName, sensorQueue] : laserQueues)
            {
                ARMARX_TRACE;
                const auto& message = sensorQueue.lookupAt(timestamp);
 
                // The lookup timestamp and the message timestamp do not match exaclty.
                // Therefore, we use the odometry information to "project" the sensor data.
                const auto odom_T_measurement = odomQueue.lookupInterpolate(message.timestamp).pose;
                const Eigen::Isometry3f measurement_T_lookup =
                    odom_T_measurement.inverse() * odom_T_lookup;
 
                LaserMessage laserData;
                laserData.data = message;
                laserData.robotPoseCorrection = measurement_T_lookup;
 
                timedData.laserData.push_back(laserData);
            }
 
            ARMARX_TRACE;
            timedDataQueue.push(timedData);
 
            // house keeping
            trimUntil(timestamp);
        }
        catch (...)
        {
            return;
        }
    }
 
    void
    ApproximateTimeQueue::trimUntil(const int64_t& timestamp)
    {
        ARMARX_TRACE;
 
        odomQueue.trimUntil(timestamp - 1'000'000);
        for (auto& [sensorName, sensorQueue] : laserQueues)
        {
            sensorQueue.trimUntil(timestamp);
        }
    }
 
    void
    ApproximateTimeQueue::serveMessageCallee()
    {
        ARMARX_TRACE;
        while (messageCalleeTask and not messageCalleeTask->isStopped())
        {
            TimedData timedData;
            if (const auto status = timedDataQueue.wait_pull(timedData);
                status == boost::queue_op_status::success)
            {
                ARMARX_TRACE;
                ARMARX_DEBUG << "Serving message callee with data at " << timedData.timestamp;
                messageCallee.onTimedDataAvailable(timedData);
            }
            else
            {
                ARMARX_WARNING << "Queue pull was not successful. "
                               << static_cast<std::underlying_type<boost::queue_op_status>::type>(
                                      status);
            }
        }
    }
 
} // namespace armarx::localization_and_mapping::cartographer_adapter