RobotHealth.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::RobotHealth
 * @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 "RobotHealth.h"
 
#include <algorithm>
#include <mutex>
#include <optional>
 
#include <boost/regex.hpp>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/Clock.h"
#include "ArmarXCore/core/time/DateTime.h"
#include "ArmarXCore/core/time/Duration.h"
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
namespace armarx
{
    inline const std::string PROPERTY_REQUESTER_ID = "required_default";
 
    void
    RobotHealth::onInitComponent()
    {
        ARMARX_TRACE;
        monitorUpdateHealthTask =
            new PeriodicTask<RobotHealth>(this, &RobotHealth::monitorHealthUpdateTaskClb, 10);
 
        defaultMaximumCycleTimeWarn =
            armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeWarnMS);
        defaultMaximumCycleTimeErr =
            armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeErrMS);
 
        const std::vector<std::string> requiredTags = simox::alg::split(p.requiredTags, ",");
        // a special requester: cannot be changed during runtime.
        tagsPerRequester[PROPERTY_REQUESTER_ID] =
            std::set<std::string>(requiredTags.begin(), requiredTags.end());
 
        setDebugObserverBatchModeEnabled(true);
    }
 
    void
    RobotHealth::onConnectComponent()
    {
        ARMARX_TRACE;
 
        monitorUpdateHealthTask->start();
    }
 
    void
    RobotHealth::monitorHealthUpdateTaskClb()
    {
        const std::scoped_lock lock(updateMutex);
 
        ARMARX_TRACE;
        auto now = armarx::core::time::DateTime::Now();
 
        for (auto& e : updateEntries)
        {
            ARMARX_TRACE;
            const std::scoped_lock elock(e.mutex);
 
            if (!e.enabled)
            {
                continue;
            }
 
            if (e.history.empty())
            {
                continue;
            }
 
            auto deltaToArrival = now - e.history.back().arrivalTime;
 
            ARMARX_TRACE;
 
            if (deltaToArrival > e.maximumCycleTimeErr)
            {
                ARMARX_TRACE;
 
                if (e.state != HealthError) // not already in error state
                {
                    ARMARX_TRACE;
                    ARMARX_ERROR << deactivateSpam(0.1, e.name) << "Component " << e.name
                                 << " has died.";
                    e.state = HealthError;
                }
            }
            else if (deltaToArrival > e.maximumCycleTimeWarn)
            {
                ARMARX_TRACE;
                ARMARX_WARNING << deactivateSpam(0.1, e.name) << "Component " << e.name
                               << " is running too slow.";
 
                e.state = HealthWarning;
            }
            else
            {
                // If everything is OK (again), update health
                e.state = HealthOK;
            }
        }
 
        // update aggregated health
        RobotHealthState overallHealthState = RobotHealthState::HealthOK;
 
 
        // get aggregated status
        for (auto& e : updateEntries)
        {
            ARMARX_TRACE;
            std::scoped_lock elock(e.mutex);
 
            // trim history
            while (e.history.size() > 20)
            {
                e.history.pop_front();
            }
 
            if (e.required)
            {
                if (e.state == HealthWarning && overallHealthState != HealthError)
                {
                    overallHealthState = RobotHealthState::HealthWarning;
                }
                else if (e.state == HealthError)
                {
                    overallHealthState = RobotHealthState::HealthError;
                }
            }
            else
            {
                if (e.state != HealthOK && overallHealthState != HealthError)
                {
                    overallHealthState = RobotHealthState::HealthWarning;
                }
            }
        }
 
        if (overallHealthState == HealthError)
        {
            ARMARX_TRACE;
            ARMARX_INFO << deactivateSpam(3) << "Requesting emergency stop";
            ARMARX_CHECK_NOT_NULL(p.emergencyStopTopicPrx);
            p.emergencyStopTopicPrx->reportEmergencyStopState(
                EmergencyStopState::eEmergencyStopActive);
        }
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(p.aggregatedRobotHealthTopicPrx);
        p.aggregatedRobotHealthTopicPrx->aggregatedHeartbeat(overallHealthState);
 
        reportDebugObserver();
    }
 
    void
    RobotHealth::onDisconnectComponent()
    {
        //robotUnitPrx = nullptr;
        monitorUpdateHealthTask->stop();
    }
 
    void
    RobotHealth::onExitComponent()
    {
    }
 
    armarx::PropertyDefinitionsPtr
    RobotHealth::createPropertyDefinitions()
    {
        auto defs = armarx::PropertyDefinitionsPtr(
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier()));
 
 
        defs->topic(p.emergencyStopTopicPrx,
                    "EmergencyStop",
                    "The name of the topic over which changes of the emergencyStopState are sent.");
        defs->topic<RobotHealthInterfacePrx>(
            p.robotHealthTopicName, "RobotHealthTopic", "Name of the RobotHealth topic");
 
        defs->topic(p.aggregatedRobotHealthTopicPrx,
                    "AggregatedRobotHealthTopic",
                    "Name of the AggregatedRobotHealthTopic");
 
        defs->optional(p.maximumCycleTimeWarnMS,
                       "MaximumCycleTimeWarnMS",
                       "Default value of the maximum cycle time for warnings");
        defs->optional(p.maximumCycleTimeErrMS,
                       "MaximumCycleTimeErrMS",
                       "Default value of the maximum cycle time for error");
 
        defs->optional(p.requiredTags, "RequiredTags", "Tags that should be requested.");
 
        return defs;
    }
 
    RobotHealth::UpdateEntry*
    RobotHealth::findUpdateEntry(const std::string& name)
    {
        ARMARX_TRACE;
 
        for (auto& updateEntrie : updateEntries)
        {
            if (updateEntrie.name == name)
            {
                return &updateEntrie;
            }
        }
 
        return nullptr;
    }
 
    std::pair<bool, RobotHealth::UpdateEntry&>
    RobotHealth::findOrCreateUpdateEntry(const std::string& name)
    {
        ARMARX_TRACE;
        {
            for (auto& updateEntrie : updateEntries)
            {
                if (updateEntrie.name == name)
                {
                    return {false, updateEntrie};
                }
            }
        }
 
        ARMARX_INFO << "registering for heartbeat: '" << name << "'";
 
        updateEntries.emplace_back(name, defaultMaximumCycleTimeWarn, defaultMaximumCycleTimeErr);
 
        return {true, updateEntries.back()};
    }
 
    void
    RobotHealth::signUp(const RobotHealthHeartbeatArgs& args, const Ice::Current& current)
    {
        ARMARX_TRACE;
        std::scoped_lock lockU(updateMutex);
 
        auto e = findOrCreateUpdateEntry(args.identifier);
 
        {
            std::scoped_lock lock(e.second.mutex);
 
            if (e.first)
            {
                ARMARX_INFO << "Newly registering component " << args.identifier << " for updates.";
            }
            // else no messsage as components may spam sign up for legacy reasons (when there was no sign up)
 
            armarx::core::time::fromIce(args.maximumCycleTimeWarning,
                                        e.second.maximumCycleTimeWarn);
            armarx::core::time::fromIce(args.maximumCycleTimeError, e.second.maximumCycleTimeErr);
 
            e.second.tags = args.tags;
            // e.second.required = args.requiredByDefault; // FIXME
            e.second.description = args.description;
            e.second.enabled = true;
            e.second.state = HealthOK;
            e.second.history.clear();
        }
 
        updateRequiredElements();
    }
 
    void
    RobotHealth::heartbeat(const std::string& identifier,
                           const core::time::dto::DateTime& referenceTime,
                           const Ice::Current& current)
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "Finding update entry";
 
        const DateTime arrivalTimestamp = Clock::Now();
 
 
        // We hold a reference to 'o' which is an element in a vector.
        // If we don't lock until the end of this scope, the vector size might change and 'o' will be invalidated.
        std::scoped_lock lockU(updateMutex);
        auto* const entry = findUpdateEntry(identifier);
 
        if (entry == nullptr)
        {
            ARMARX_WARNING << deactivateSpam() << "Attention. Component `" << identifier
                           << "` was not signed up for heartbeat. Ignoring heartbeat for now...";
            return;
        }
 
        ARMARX_CHECK_NOT_NULL(entry);
 
        std::scoped_lock lock(entry->mutex);
 
        if (not entry->enabled)
        {
            ARMARX_WARNING << deactivateSpam() << "Attention. Component `" << identifier
                           << "` was not enabled. The heartbeat is ignored... ";
            return;
        }
 
        ARMARX_VERBOSE << "Extending history";
        // auto now = armarx::core::time::DateTime::Now();
 
 
        armarx::core::time::DateTime refTime;
        fromIce(referenceTime, refTime);
        entry->history.push_back(
            UpdateEntry::TimeInfo{.referenceTime = refTime, .arrivalTime = arrivalTimestamp});
    }
 
    void
    armarx::RobotHealth::unregister(const std::string& identifier, const Ice::Current&)
    {
        ARMARX_TRACE;
        std::scoped_lock lock(updateMutex);
 
        bool found = false;
        {
            for (auto& e : updateEntries)
            {
                if (e.name == identifier)
                {
                    std::scoped_lock elock(e.mutex);
                    e.required = false;
                    e.enabled = false;
                    found = true;
                    break;
                }
            }
            // Todo upgrade lock?
        }
 
        if (found)
        {
            ARMARX_INFO << "Component " << identifier << " disabled from heartbeat";
        }
        else
        {
            ARMARX_INFO << "Could not unregister component " << identifier << ". Not found.";
        }
 
        updateRequiredElements();
    }
 
    void
    armarx::RobotHealth::addRequiredTags(const std::string& requesterIdentifier,
                                         const std::vector<std::string>& tags,
                                         const Ice::Current& current)
    {
        std::scoped_lock lock(updateMutex);
 
        // add newly requested tags
        for (const auto& tag : tags)
        {
            tagsPerRequester[requesterIdentifier].insert(tag);
        }
 
        updateRequiredElements();
    }
 
    std::set<std::string>
    armarx::RobotHealth::requestedTags() const
    {
        // obtain a list of all tags that are relevant / requested at the moment
        std::set<std::string> allRequestedTags;
        for (const auto& tgs : simox::alg::get_values(tagsPerRequester))
        {
            for (const auto& tag : tgs)
            {
                allRequestedTags.insert(tag);
            }
        }
        return allRequestedTags;
    }
 
    // checks whether any element in 'search' is in 'values'
    bool
    containsAnyOf(const std::set<std::string>& values, const std::vector<std::string>& searchKeys)
    {
        return std::any_of(searchKeys.begin(),
                           searchKeys.end(),
                           [&values](const auto& key) { return values.count(key) > 0; });
    }
 
    void
    armarx::RobotHealth::updateRequiredElements()
    {
        ARMARX_IMPORTANT << "updateRequiredElements";
        const std::set<std::string> allRequestedTags = requestedTags();
 
        for (auto& e : updateEntries)
        {
            ARMARX_INFO << e.name;
 
            e.required = false; // if required, will be set in the following
 
            std::unique_lock lock(e.mutex);
 
            if (containsAnyOf(allRequestedTags, e.tags))
            {
                ARMARX_INFO << e.name << " is required.";
                e.required = true;
                if (not e.enabled)
                {
                    ARMARX_WARNING << "You are requiring the disabled component " << e.name
                                   << ". Enabling it...";
                    e.enabled = true;
                }
            }
        }
    }
 
    void
    armarx::RobotHealth::removeRequiredTags(const std::string& requesterIdentifier,
                                            const std::vector<std::string>& tags,
                                            const Ice::Current& current)
    {
        std::scoped_lock lock(updateMutex);
 
        // update the required tags list / map
        for (const auto& tag : tags)
        {
            tagsPerRequester[requesterIdentifier].erase(tag);
        }
 
        updateRequiredElements();
    }
 
    void
    armarx::RobotHealth::resetRequiredTags(const Ice::Current& current)
    {
        std::scoped_lock lock(updateMutex);
 
        // treat the special provider (see onInitComponent()) appropriately
        ARMARX_CHECK(tagsPerRequester.count(PROPERTY_REQUESTER_ID) > 0);
        const auto propertyProviderTags = tagsPerRequester.at(PROPERTY_REQUESTER_ID);
 
        // clear everything except the special provider
        tagsPerRequester.clear();
        tagsPerRequester.emplace(PROPERTY_REQUESTER_ID, propertyProviderTags);
    }
 
    RobotHealthInfo
    RobotHealth::getSummary(const Ice::Current&)
    {
        std::scoped_lock lock(updateMutex);
 
        ARMARX_TRACE;
        RobotHealthInfo ret;
 
        auto now = armarx::core::time::DateTime::Now();
 
        armarx::core::time::Duration minDelta;
        armarx::core::time::Duration maxDelta;
 
        for (const auto& e : updateEntries)
        {
            RobotHealthInfoEntry healthEntry;
 
            for (size_t i = 1; i < e.history.size(); i++)
            {
                auto later = e.history[i];
                auto pre = e.history[i - 1];
 
                auto delta = later.arrivalTime - pre.arrivalTime;
 
                if (minDelta > delta)
                {
                    minDelta = delta;
                }
 
                if (maxDelta < delta)
                {
                    maxDelta = delta;
                }
            }
 
            const Duration timeSinceLastUpdateArrival =
                e.history.empty() ? Duration() : Clock::Now() - e.history.back().arrivalTime;
            const Duration timeSinceLastUpdateReference =
                e.history.empty() ? Duration() : Clock::Now() - e.history.back().referenceTime;
 
            const DateTime lastReferenceTime = e.history.empty()
                                                   ? armarx::core::time::DateTime::Invalid()
                                                   : e.history.back().referenceTime;
 
            const Duration timeSyncDelayAndIce =
                e.history.empty() ? armarx::core::time::Duration()
                                  : e.history.back().arrivalTime - e.history.back().referenceTime;
 
            healthEntry.identifier = e.name;
            healthEntry.state = e.state;
            healthEntry.enabled = e.enabled;
            healthEntry.required = e.required;
            toIce(healthEntry.minDelta, minDelta);
            toIce(healthEntry.maxDelta, maxDelta);
            toIce(healthEntry.lastReferenceTimestamp, lastReferenceTime);
            toIce(healthEntry.timeSinceLastArrival, timeSinceLastUpdateArrival);
            toIce(healthEntry.timeSyncDelayAndIce, timeSyncDelayAndIce);
            toIce(healthEntry.timeSinceLastUpdateReference, timeSinceLastUpdateReference);
            toIce(healthEntry.maximumCycleTimeWarning, e.maximumCycleTimeWarn);
            toIce(healthEntry.maximumCycleTimeError, e.maximumCycleTimeErr);
            healthEntry.tags = e.tags;
 
            ret.entries[e.name] = healthEntry;
        }
 
        const auto tags = requestedTags();
        ret.activeTags = std::vector<std::string>(tags.begin(), tags.end());
 
        return ret;
    }
 
    std::string
    RobotHealth::getTopicName(const Ice::Current& current)
    {
        return p.robotHealthTopicName;
    }
 
    void
    RobotHealth::reportDebugObserver()
    {
        for (const auto& entry : updateEntries)
        {
            if (entry.history.size() > 1)
            {
                auto later = entry.history[entry.history.size() - 1];
                auto pre = entry.history[entry.history.size() - 2];
                const Duration delta = later.arrivalTime - pre.arrivalTime;
 
                const Duration timeSinceLastArrival =
                    Clock::Now() - entry.history.back().arrivalTime;
                const Duration timeToLastReference =
                    Clock::Now() - entry.history.back().referenceTime;
                const Duration timeSyncDelay =
                    entry.history.back().arrivalTime - entry.history.back().referenceTime;
 
                setDebugObserverDatafield("RobotHealth_" + entry.name + "_lastDelta",
                                          delta.toMilliSecondsDouble());
                setDebugObserverDatafield("RobotHealth_" + entry.name + "_timeSinceLastArrival",
                                          timeSinceLastArrival.toMilliSecondsDouble());
                setDebugObserverDatafield("RobotHealth_" + entry.name + "_timeToLastReference",
                                          timeToLastReference.toMilliSecondsDouble());
                setDebugObserverDatafield("RobotHealth_" + entry.name + "_timeSyncDelayAndIce",
                                          timeSyncDelay.toMilliSecondsDouble());
                setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeWarn",
                                          entry.maximumCycleTimeWarn.toMilliSecondsDouble());
                setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeErr",
                                          entry.maximumCycleTimeErr.toMilliSecondsDouble());
            }
        }
 
        sendDebugObserverBatch();
    }
} // namespace armarx