aron_conversions.cpp

Go to the documentation of this file.

#include "aron_conversions.h"
 
#include <algorithm>
#include <cstdint>
#include <iterator>
 
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
 
#include "types.h"
 
namespace armarx::armem::laser_scans
{
 
    /************ fromAron ************/
    SensorHeader
    fromAron(const arondto::SensorHeader& aronSensorHeader)
    {
        return {.agent = aronSensorHeader.agent,
                .frame = aronSensorHeader.frame,
                .timestamp = aron::fromAron<Time>(aronSensorHeader.timestamp)};
    }
 
    void
    fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan)
    {
        laserScan.header = fromAron(aronLaserScan.header);
        // laserScan.data = fromAron(aronLaserScan.data);
    }
 
    void
    fromAron(const arondto::LaserScanStamped& aronLaserScan,
             LaserScan& laserScan,
             std::int64_t& timestamp,
             std::string& frame,
             std::string& agentName)
    {
        const auto header = fromAron(aronLaserScan.header);
 
        // laserScan = fromAron(aronLaserScan.data);
 
        timestamp = header.timestamp.toMicroSecondsSinceEpoch();
        frame = header.frame;
        agentName = header.agent;
    }
 
    /************ toAron ************/
 
    // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
    // {
    //     aronLaserScan.scan = toAron(laserScan);
    // }
 
    int64_t
    toAron(const armem::Time& timestamp)
    {
        return timestamp.toMicroSecondsSinceEpoch();
    }
 
    arondto::SensorHeader
    toAron(const SensorHeader& sensorHeader)
    {
        arondto::SensorHeader aronSensorHeader;
 
        aronSensorHeader.agent = sensorHeader.agent;
        aronSensorHeader.frame = sensorHeader.frame;
        aron::toAron(aronSensorHeader.timestamp, sensorHeader.timestamp);
 
        return aronSensorHeader;
    }
 
    void
    toAron(const LaserScanStamped& laserScanStamped,
           arondto::LaserScanStamped& aronLaserScanStamped)
    {
        aronLaserScanStamped.header = toAron(laserScanStamped.header);
        // toAron(laserScanStamped.data, aronLaserScanStamped.data);
    }
 
    void
    toAron(const LaserScan& laserScan,
           const armem::Time& timestamp,
           const std::string& frame,
           const std::string& agentName,
           arondto::LaserScanStamped& aronLaserScanStamped)
    {
        const SensorHeader header{.agent = agentName, .frame = frame, .timestamp = timestamp};
 
        const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
 
        toAron(laserScanStamped, aronLaserScanStamped);
    }
 
 
} // namespace armarx::armem::laser_scans