LaserScannerTimestampTest.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotComponents::ArmarXObjects::laser_scanner_timestamp_test
 * @author     Corvin-N ( corvin at navarro dot de )
 * @date       2023
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "LaserScannerTimestampTest.h"
 
#include <cstddef>
 
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
namespace RobotComponents
{
 
    const std::string Component::defaultName = "LaserScannerTimestampTest";
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->optional(
            properties.topicName, "TopicName", "Name of the laserscanner topic to report to.");
 
        def->component(laserScanUnitListenerPrx);
 
 
        def->optional(properties.useTopic, "useTopic", "If enabled, will send data via topic.");
 
        def->required(properties.frame, "frame", "Coordinate frame of the laserscanner.");
 
        def->required(properties.freq,
                      "frequency",
                      "Default frequency of newly generated and published laser scans [Hz]");
 
        def->required(
            properties.min,
            "minOffset",
            "Smallest allowed offset of wrong timestamps [ms]. May be negative. Offset for wrong "
            "generated timestamp randomly chosen from interval [minOffset,maxOffset]");
        def->required(
            properties.max,
            "maxOffset",
            "Greatest allowed offset of wrong timestamps [ms]. May be negative. Offset for wrong "
            "generated timestamp randomly chosen from interval [minOffset,maxOffset]");
 
        def->required(
            properties.correctBatchSize,
            "MaxCorrectBatchSize",
            "Maximum size of consecutive generated correct timestamps. Batch size of correct "
            "timestamps is randomly chosen from interval [1,MaxCorrectBatchSize]");
        def->required(properties.wrongBatchSize,
                      "MaxWrongBatchSize",
                      "Maximum size of consecutive generated wrong timestamps. Batch size of wrong "
                      "timestamps is randomly chosen from interval [1,MaxWrongBatchSize");
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        if (properties.min >= properties.max)
        {
            ARMARX_ERROR << "following statement should be false but is not: min >= max";
        }
 
        if (properties.correctBatchSize < 1)
        {
            ARMARX_ERROR << "following statement should be false but is not: correctBatchSize < 1";
        }
 
        if (properties.wrongBatchSize < 1)
        {
            ARMARX_ERROR << "following statement should be false but is not: wrongBatchSize < 1";
        }
 
        // optional topic
        if (properties.useTopic)
        {
            offeringTopic(properties.topicName);
        }
    }
 
    void
    Component::onConnectComponent()
    {
        // Do things after connecting to topics and components.
 
        if (properties.useTopic)
        {
            getTopic(laserScanUnitListenerTopic, properties.topicName);
        }
 
        runTask = new armarx::RunningTask<Component>(this, &Component::run);
        runTask->start();
    }
 
    void
    Component::run()
    {
        ARMARX_IMPORTANT << "Started running task!!!";
 
        int scansUntilToggle = randInRange(1, properties.correctBatchSize);
        bool correctScans = true;
 
        armarx::Metronome metronome(armarx::Frequency::Hertz(properties.freq));
        while (true)
        {
            constexpr std::size_t nSamples = 1000;
 
            armarx::LaserScan scanData;
            for (std::size_t i = 0; i < nSamples; i++)
            {
                scanData.emplace_back(armarx::LaserScanStep{2 * M_PI / nSamples, 2500.0F, 1.0f});
            }
            armarx::DateTime scanTime = armarx::DateTime::Now();
 
            if (!correctScans)
            {
                armarx::Duration scanOffset =
                    armarx::Duration::MilliSeconds(randInRange(properties.min, properties.max));
                scanTime += scanOffset;
                ARMARX_INFO << "Using offset of " << scanOffset.toMilliSeconds() << "ms.";
            }
 
            armarx::TimestampVariantPtr scanT(new armarx::TimestampVariant(scanTime));
 
 
            if (laserScanUnitListenerTopic)
            {
                laserScanUnitListenerTopic->reportSensorValues(
                    properties.frame, properties.frame, scanData, scanT);
            }
 
            // if(laserScanUnitListenerPrx)
            // {
            //     laserScanUnitListenerPrx->reportSensorValues(properties.frame, properties.frame, scanData, scanT);
            // }
 
            scansUntilToggle--;
            if (scansUntilToggle <= 0)
            {
                if (correctScans)
                {
                    scansUntilToggle = randInRange(1, properties.wrongBatchSize);
                    ARMARX_INFO << "Now sending wrong timestamps";
                }
                else
                {
                    scansUntilToggle = randInRange(1, properties.correctBatchSize);
                    ARMARX_INFO << "Now sending correct timestamps";
                }
                correctScans = !correctScans;
            }
 
            metronome.waitForNextTick();
        }
    }
 
    int
    Component::randInRange(int min, int max)
    {
        int size = (max + 1) - min;
        int rand = std::rand();
 
        return (rand % size) + min;
    }
 
    void
    Component::onDisconnectComponent()
    {
        runTask->stop();
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    std::string
    Component::getDefaultName() const
    {
        return Component::defaultName;
    }
 
    std::string
    Component::GetDefaultName()
    {
        return Component::defaultName;
    }
 
} // namespace RobotComponents