SkillEventRenderer.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::OpossumLogger
 * @author     Joana Plewnia ( joana dot plewnia at kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SkillEventRenderer.h"
 
#include <algorithm>
#include <cctype>
#include <cstdio>
#include <ctime>
#include <map>
 
#include <SimoxUtility/algorithm/string.h>
 
namespace armarx::opossum
{
 
    namespace
    {
 
        /// The whitespace characters Python's `str.split()` splits on.
        const std::string Whitespace = " \t\n\r\f\v";
 
        bool
        isBlank(const std::string& text)
        {
            return text.find_first_not_of(Whitespace) == std::string::npos;
        }
 
        /// `" a  b\n c "` -> `"a b c"`, like `" ".join(text.split())`.
        std::string
        collapseWhitespace(const std::string& text)
        {
            return simox::alg::join(simox::alg::split(text, Whitespace, true, true), " ");
        }
 
        /// The part of `text` before the first occurrence of `separator`.
        std::string
        before(const std::string& text, const std::string& separator)
        {
            const std::size_t pos = text.find(separator);
            return pos == std::string::npos ? text : text.substr(0, pos);
        }
 
        /// The part of `text` after the last occurrence of `separator`.
        std::string
        afterLast(const std::string& text, const std::string& separator)
        {
            const std::size_t pos = text.rfind(separator);
            return pos == std::string::npos ? text : text.substr(pos + separator.size());
        }
 
        std::string
        stringOr(const nlohmann::json& value, const std::string& fallback = "")
        {
            return value.is_string() ? value.get<std::string>() : fallback;
        }
 
        /// Datasets from open-vocabulary detectors: the className is already
        /// natural language ("glass cup") and needs no lookup.
        const std::set<std::string>&
        openVocabularyDatasets()
        {
            static const std::set<std::string> datasets{"grounding_sam", "OpenVocabulary"};
            return datasets;
        }
 
        /// How a parameter value is turned into text.
        enum class Kind
        {
            /// Free text, gets quoted.
            Quoted,
            /// An ObjectID, or a plain object name.
            Object,
            /// A location string `dataset/class/spot`.
            Location,
            /// Any other string worth humanizing.
            Humanize
        };
 
        struct Subject
        {
            const char* field;
            std::vector<const char*> keys;
            Kind kind;
        };
 
        /**
         * Parameter keys carry consistent meaning across skills, so map
         * key -> field generically rather than per skill. At most one value per
         * field; the first matching key wins.
         */
        const std::vector<Subject>&
        subjects()
        {
            // NB: `tableIdForGarmentPickup` is deliberately absent -- that is the
            // surface a garment is picked from, not the object being acted on.
            static const std::vector<Subject> subjects{
                {"text", {"text"}, Kind::Quoted},
                {"question", {"question"}, Kind::Quoted},
                {"object",
                 {"graspObjectName",
                  "object",
                  "objectID",
                  "objectInstanceID",
                  "objectToGrasp",
                  "cartObjectId",
                  "cartObjectID",
                  "object1",
                  "instance_id"},
                 Kind::Object},
                {"query",
                 {"objectName",
                  "objectText",
                  "basketDetectionQuery",
                  "familiarObjectDetectionQuery",
                  "objectClass",
                  "discovery.objectClass"},
                 Kind::Quoted},
                {"location",
                 {"location",
                  "locationName",
                  "graspLocationName",
                  "handoverLocationName",
                  "placementLocationName",
                  "navigateToFridgeLocation",
                  "navigateToHandoverLocation",
                  "graspNextGarmentLocationName",
                  "placeGarmentInBasketLocationName",
                  "graspObjectAtLocation"},
                 Kind::Location},
                {"place", {"commonPlace"}, Kind::Location},
                {"part", {"objectToInteract", "specificNode", "objectFrame", "node"}, Kind::Humanize},
            };
            return subjects;
        }
 
    } // namespace
 
    namespace detail
    {
 
        nlohmann::json
        flat(const nlohmann::json& node)
        {
            if (node.is_array())
            {
                nlohmann::json out = nlohmann::json::array();
                for (const auto& element : node)
                {
                    out.push_back(flat(element));
                }
                return out;
            }
            if (not node.is_object())
            {
                return node;
            }
 
            if (const auto value = node.find("_ARON_VALUE"); value != node.end())
            {
                return *value;
            }
 
            // `_ARON_ELEMENTS` may hold either an object (dict) or an array (list).
            if (const auto elements = node.find("_ARON_ELEMENTS"); elements != node.end())
            {
                if (elements->is_array())
                {
                    nlohmann::json out = nlohmann::json::array();
                    for (const auto& element : *elements)
                    {
                        out.push_back(flat(element));
                    }
                    return out;
                }
                nlohmann::json out = nlohmann::json::object();
                for (auto it = elements->begin(); it != elements->end(); ++it)
                {
                    out[it.key()] = flat(it.value());
                }
                return out;
            }
 
            return nlohmann::json();
        }
 
        std::string
        humanize(const std::string& text)
        {
            // Split camelCase, lowercasing the boundary (`laundryBasket` ->
            // `laundry basket`) but keeping a leading capital (`Fridge_handle`
            // -> `Fridge handle`).
            std::string split;
            split.reserve(text.size() + 8);
            for (std::size_t i = 0; i < text.size(); ++i)
            {
                const unsigned char c = static_cast<unsigned char>(text[i]);
                const unsigned char previous =
                    i > 0 ? static_cast<unsigned char>(text[i - 1]) : '\0';
                if (std::isupper(c) and (std::islower(previous) or std::isdigit(previous)))
                {
                    split += ' ';
                    split += static_cast<char>(std::tolower(c));
                }
                else
                {
                    split += text[i];
                }
            }
 
            // Replace runs of `-` and `_` by a single space.
            std::string separated;
            separated.reserve(split.size());
            bool inSeparator = false;
            for (const char c : split)
            {
                if (c == '-' or c == '_')
                {
                    inSeparator = true;
                    continue;
                }
                if (inSeparator)
                {
                    separated += ' ';
                    inSeparator = false;
                }
                separated += c;
            }
            if (inSeparator)
            {
                separated += ' ';
            }
 
            return simox::alg::trim_copy(separated);
        }
 
        bool
        isObjectID(const nlohmann::json& value)
        {
            return value.is_object() and value.contains("className") and value.contains("dataset") and
                   value.contains("instanceName");
        }
 
        const nlohmann::json*
        lookup(const nlohmann::json& parameters, const std::string& key)
        {
            if (not parameters.is_object())
            {
                return nullptr;
            }
            if (const std::size_t dot = key.find('.'); dot != std::string::npos)
            {
                const auto outer = parameters.find(key.substr(0, dot));
                return outer == parameters.end() ? nullptr : lookup(*outer, key.substr(dot + 1));
            }
            const auto value = parameters.find(key);
            return value == parameters.end() ? nullptr : &(*value);
        }
 
        std::string
        errorText(const std::string& message)
        {
            if (message.empty())
            {
                return "";
            }
 
            static const std::string reasonMarker = "Reason:";
            std::string text;
            if (const std::size_t reasonStart = message.find(reasonMarker);
                reasonStart != std::string::npos)
            {
                // The `Reason:` line is prefixed with a full C++ signature, and is
                // followed by a backtrace we do not want.
                const std::string reason = simox::alg::trim_copy(
                    before(message.substr(reasonStart + reasonMarker.size()), "\nBacktrace"));
                text = collapseWhitespace(afterLast(reason, ": "));
            }
            else
            {
                // The first line is often just "Caught armarx::LocalException:", and
                // 501 messages trail off into an Ice dump after "The error was:".
                text = collapseWhitespace(before(before(message, "The error was:"), "\n"));
            }
 
            if (text.size() > 100)
            {
                const std::string head = text.substr(0, 100);
                const std::size_t space = head.rfind(' ');
                text = (space == std::string::npos ? head : head.substr(0, space)) + "...";
            }
            return text;
        }
 
        std::string
        quoted(const std::string& text)
        {
            std::string escaped;
            escaped.reserve(text.size() + 2);
            for (const char c : text)
            {
                escaped += (c == '"') ? '\'' : (c == '\n' ? ' ' : c);
            }
            return "\"" + escaped + "\"";
        }
 
        std::optional<std::string>
        stateOf(const std::string& status)
        {
            static const std::map<std::string, std::string> states{{"Running", "started"},
                                                                   {"Succeeded", "done"},
                                                                   {"Failed", "failed"},
                                                                   {"Aborted", "aborted"}};
            const auto state = states.find(status);
            return state == states.end() ? std::nullopt : std::optional{state->second};
        }
 
        std::string
        calledBy(const std::string& executorName)
        {
            return afterLast(afterLast(executorName, "->"), "/");
        }
 
        std::string
        timestamp(const armarx::core::time::DateTime& time)
        {
            // Local time with truncated (not rounded) milliseconds, matching
            // Python's `datetime.fromtimestamp(...).isoformat(timespec="milliseconds")`.
            const std::int64_t microSeconds = time.toMicroSecondsSinceEpoch();
            const std::time_t seconds = static_cast<std::time_t>(microSeconds / 1'000'000);
            const int milliSeconds = static_cast<int>((microSeconds % 1'000'000) / 1'000);
 
            std::tm local{};
            localtime_r(&seconds, &local);
 
            char buffer[32];
            if (std::strftime(buffer, sizeof(buffer), "%Y-%m-%dT%H:%M:%S", &local) == 0)
            {
                return "";
            }
 
            char result[40];
            std::snprintf(result, sizeof(result), "%s.%03d", buffer, milliSeconds);
            return result;
        }
 
    } // namespace detail
 
    SkillEventRenderer::SkillEventRenderer() : SkillEventRenderer(DefaultNoiseSkills(), nullptr)
    {
    }
 
    SkillEventRenderer::SkillEventRenderer(std::set<std::string> noiseSkills,
                                           SpokenNameLookup spokenNameLookup) :
        noiseSkills(std::move(noiseSkills)), spokenNameLookup(std::move(spokenNameLookup))
    {
    }
 
    const std::set<std::string>&
    SkillEventRenderer::DefaultNoiseSkills()
    {
        // Pure body/gaze control, plus primitives that only implement a skill
        // already logged above them (LookAt under LookAtObject, CommonActions
        // under GraspCup). These would swamp the summary.
        static const std::set<std::string> noise{
            "ShapeHand",
            "CloseHand",
            "OpenHand",
            "MoveJointsToPosition",
            "MoveJointsToNamedConfiguration",
            "MoveJointsWithVelocity",
            "SetKneeHipPosition",
            "HomePose",
            "SafeHomePose",
            "MoveArmsToDefaultHighPosition",
            "MoveArmsAboveBelowTable",
            "SwitchCoordinationMode",
            "ZeroTorque",
            "ResetGazeTargets",
            "SetCustomGazeTarget",
            "LookAhead",
            "LookAlongTrajectory",
            "LookAt",
            "LookDown",
            "LookDownstraight",
            "LookLeft",
            "LookRight",
            "CommonActions",
            "Placing::AnyObject::AtPose",
            "grasping_failure_execute_encoder_calibration",
        };
        return noise;
    }
 
    std::string
    SkillEventRenderer::DefaultNoiseSkillsString()
    {
        const std::set<std::string>& noise = DefaultNoiseSkills();
        return simox::alg::join(std::vector<std::string>(noise.begin(), noise.end()), ",");
    }
 
    std::set<std::string>
    SkillEventRenderer::ParseSkillList(const std::string& commaSeparated)
    {
        const std::vector<std::string> names = simox::alg::split(commaSeparated, ",", true, true);
        return std::set<std::string>(names.begin(), names.end());
    }
 
    std::string
    SkillEventRenderer::objectName(const nlohmann::json& objectID) const
    {
        if (not detail::isObjectID(objectID))
        {
            return "";
        }
        const std::string className = stringOr(objectID.at("className"));
        if (className.empty())
        {
            return "";
        }
 
        const std::string dataset = stringOr(objectID.at("dataset"));
        if (openVocabularyDatasets().count(dataset) > 0)
        {
            return className;
        }
 
        std::optional<std::string> spokenName;
        if (spokenNameLookup)
        {
            spokenName = spokenNameLookup(dataset, className);
        }
        // Humanize the spoken name too: some newer entries store the raw
        // camelCase className as the spoken name (`IKEA/laundryBasket`).
        return detail::humanize(spokenName.value_or(className));
    }
 
    std::string
    SkillEventRenderer::locationName(const std::string& value) const
    {
        const std::string text = before(value, ":");
        const std::vector<std::string> parts = simox::alg::split(text, "/", false, false);
        if (parts.size() < 3)
        {
            return detail::humanize(text);
        }
 
        const std::string spot =
            simox::alg::join(std::vector<std::string>(parts.begin() + 2, parts.end()), "/");
        const nlohmann::json objectID = {
            {"dataset", parts[0]}, {"className", parts[1]}, {"instanceName", ""}};
        return detail::humanize(spot) + " of " + objectName(objectID);
    }
 
    std::vector<std::string>
    SkillEventRenderer::subjectFields(const nlohmann::json& parameters) const
    {
        std::vector<std::string> fields;
        for (const Subject& subject : subjects())
        {
            for (const char* key : subject.keys)
            {
                const nlohmann::json* value = detail::lookup(parameters, key);
                if (value == nullptr)
                {
                    continue;
                }
 
                const std::string text = stringOr(*value);
                std::string rendered;
                switch (subject.kind)
                {
                    case Kind::Quoted:
                        if (value->is_string() and not isBlank(text))
                        {
                            rendered = detail::quoted(text);
                        }
                        break;
                    case Kind::Object:
                        if (detail::isObjectID(*value))
                        {
                            rendered = objectName(*value);
                        }
                        else if (value->is_string() and not isBlank(text))
                        {
                            // Some skills name the object directly ("apple juice").
                            rendered = detail::humanize(text);
                        }
                        break;
                    case Kind::Location:
                        if (value->is_string() and not isBlank(text))
                        {
                            rendered = locationName(text);
                        }
                        break;
                    case Kind::Humanize:
                        if (value->is_string() and not isBlank(text))
                        {
                            rendered = detail::humanize(text);
                        }
                        break;
                }
 
                if (not rendered.empty())
                {
                    fields.push_back(std::string(subject.field) + ":" + rendered);
                    break;
                }
            }
        }
        return fields;
    }
 
    bool
    SkillEventRenderer::isNoiseSkill(const std::string& skillName) const
    {
        return noiseSkills.count(skillName) > 0;
    }
 
    std::optional<std::string>
    SkillEventRenderer::render(const SkillEvent& event) const
    {
        const std::optional<std::string> state = detail::stateOf(event.status);
        if (not state.has_value() or isNoiseSkill(event.skillName))
        {
            return std::nullopt;
        }
 
        const nlohmann::json parameters = detail::flat(event.parameters);
        const nlohmann::json result = detail::flat(event.result);
 
        std::vector<std::string> fields{"skill:" + event.skillName};
        for (std::string& field : subjectFields(parameters))
        {
            fields.push_back(std::move(field));
        }
        // executorName is the call stack; its last segment is the caller.
        fields.push_back("called_by:" + detail::calledBy(event.executorName));
        fields.push_back("state:" + *state);
        fields.push_back("time_stamp:" + detail::timestamp(event.timestamp));
 
        if (*state == "failed" or *state == "aborted")
        {
            const nlohmann::json* errorMessage =
                result.is_object() ? detail::lookup(result, "errorMessage") : nullptr;
            const std::string reason =
                detail::errorText(errorMessage == nullptr ? "" : stringOr(*errorMessage));
            if (not reason.empty())
            {
                fields.push_back("error:" + reason);
            }
        }
 
        return simox::alg::join(fields, ", ");
    }
 
} // namespace armarx::opossum