RobotHealthDummy.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    RobotAPI::ArmarXObjects::RobotHealthDummy
 * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RobotHealthDummy.h"
 
#include <time.h>
 
#include <thread>
 
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
namespace armarx
{
    void
    RobotHealthDummy::onInitComponent()
    {
        dummyTask = new RunningTask<RobotHealthDummy>(this, &RobotHealthDummy::runTask);
        getProperty(sleepmode, "SleepMode");
    }
 
    void
    RobotHealthDummy::onConnectComponent()
    {
        robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>(
            getProperty<std::string>("RobotHealthTopicName").getValue());
        dummyTask->start();
    }
 
#define NANOSECS_PER_SEC 1000000000
 
    int
    RobotHealthDummy::NanoSleep(long nanosec)
    {
        struct timespec ts;
        ts.tv_sec = nanosec / NANOSECS_PER_SEC;
        ts.tv_nsec = (nanosec % NANOSECS_PER_SEC);
        return nanosleep(&ts, nullptr); // jitter up to +100ms!
    }
 
    void
    RobotHealthDummy::sleepwait(long microseconds)
    {
        long start = TimeUtil::GetTime().toMicroSeconds();
        auto end = start + microseconds;
        do
        {
            NanoSleep(1000);
        } while (TimeUtil::GetTime().toMicroSeconds() < end);
    }
 
    void
    RobotHealthDummy::yieldwait(long microseconds)
    {
        long start = TimeUtil::GetTime().toMicroSeconds();
        auto end = start + microseconds;
        do
        {
            std::this_thread::yield();
        } while (TimeUtil::GetTime().toMicroSeconds() < end); // jitter up to +25ms...
    }
 
    void
    RobotHealthDummy::busywait(long microseconds)
    {
        long start = TimeUtil::GetTime().toMicroSeconds();
        auto end = start + microseconds;
        do
        {
            ;
        } while (TimeUtil::GetTime().toMicroSeconds() < end);
    }
 
    void
    RobotHealthDummy::runTask()
    {
        auto args = RobotHealthHeartbeatArgs();
        args.identifier = getName();
        armarx::core::time::toIce(args.maximumCycleTimeError, armarx::core::time::Duration::MilliSeconds(1000));
        armarx::core::time::toIce(args.maximumCycleTimeWarning, armarx::core::time::Duration::MilliSeconds(500));
        robotHealthTopicPrx->signUp(args);
 
        ARMARX_INFO << "starting rinning task";
        while (!dummyTask->isStopped())
        {
            auto beforeTopicCall = armarx::core::time::DateTime::Now();
            //ARMARX_INFO << "send heartbeat";
 
            armarx::core::time::dto::DateTime now;
            armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
            robotHealthTopicPrx->heartbeat(getName(), now);
            auto afterTopicCall = armarx::core::time::DateTime::Now();
 
            if (sleepmode == "nanosleep")
            {
                NanoSleep(10 * 1000 * 1000); // wait 10 milliseconds
            }
            else if (sleepmode == "sleepwait")
            {
                sleepwait(10 * 1000); // wait 10 milliseconds
            }
            else if (sleepmode == "yieldwait")
            {
                yieldwait(10 * 1000); // wait 10 milliseconds
            }
            else if (sleepmode == "busywait")
            {
                busywait(10 * 1000); // wait 10 milliseconds
            }
            else if (sleepmode == "std::this_thread::sleep_for")
            {
                std::this_thread::sleep_for(std::chrono::microseconds(10 * 1000));
            }
            else
            {
                throw LocalException("Unknown sleepmode.");
            }
 
            auto afterSleep = armarx::core::time::DateTime::Now();
            auto topicCallDelay = afterTopicCall - beforeTopicCall;
            auto sleepDelay = afterSleep - afterTopicCall;
            if (sleepDelay.toMicroSeconds() > 20000)
            {
                ARMARX_IMPORTANT << sleepmode << " took long: " << sleepDelay << "us";
            }
            if (topicCallDelay.toMicroSeconds() > 1000)
            {
                ARMARX_IMPORTANT << "topic took long: " << topicCallDelay << "us";
            }
        }
 
        robotHealthTopicPrx->unregister(args.identifier);
    }
 
    void
    RobotHealthDummy::onDisconnectComponent()
    {
        //ARMARX_IMPORTANT << "onDisconnectComponent";
        dummyTask->stop();
    }
 
    void
    RobotHealthDummy::onExitComponent()
    {
        //ARMARX_IMPORTANT << "onExitComponent";
        dummyTask->stop();
    }
 
    armarx::PropertyDefinitionsPtr
    RobotHealthDummy::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
            new RobotHealthDummyPropertyDefinitions(getConfigIdentifier()));
    }
} // namespace armarx