OpossumLogger.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 "OpossumLogger.h"
 
#include <csignal>
 
#include <pthread.h>
 
#include <algorithm>
#include <fstream>
#include <map>
#include <optional>
#include <utility>
 
#include <SimoxUtility/json/json.hpp>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
 
// cpp-httplib, vendored for the ArMem REST persistence strategy.
#include <RobotAPI/libraries/armem/server/ltm/detail/mixins/util/httplib.h>
 
namespace armarx
{
 
    namespace
    {
        /// How many snapshot IDs to remember for duplicate detection.
        constexpr std::size_t SeenCapacity = 512;
 
        /// How long to keep draining the queue when shutting down.
        constexpr std::chrono::milliseconds DrainTimeout{1000};
    } // namespace
 
    OpossumLogger::~OpossumLogger()
    {
        // ~SubscriptionHandle does not release by itself, so without this the
        // memory listener would keep calling onSkillEventUpdate() on a
        // half-destroyed component whenever onDisconnectComponent() did not run.
        // Releasing twice is safe: MemoryListener::unsubscribe() guards on the
        // handle's `valid` flag.
        subscription.release();
        stopSender();
    }
 
    std::string
    OpossumLogger::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    std::string
    OpossumLogger::GetDefaultName()
    {
        return "OpossumLogger";
    }
 
    armarx::PropertyDefinitionsPtr
    OpossumLogger::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->optional(p.enabled, "opossum.Enabled", "Whether to report skill events at all.");
        defs->optional(p.robotName,
                       "opossum.RobotName",
                       "Value of the 'robot_name' field. The server sorts the logs of the "
                       "different robots by this name.");
        defs->optional(p.host, "opossum.Host", "Host of the OPOSSUM summary server.");
        defs->optional(p.port, "opossum.Port", "Port of the OPOSSUM summary server.");
        defs->optional(p.path,
                       "opossum.Path",
                       "Path the log lines are POSTed to (without leading slash).");
        defs->optional(p.timeoutMs,
                       "opossum.TimeoutMs",
                       "Connect, read and write timeout of a single POST in milliseconds.");
        defs->optional(p.queueSize,
                       "opossum.QueueSize",
                       "Maximal number of pending lines. When the server is slow or "
                       "unreachable, the oldest lines are dropped.");
        defs->optional(p.outputFile,
                       "opossum.OutputFile",
                       "If set, every reported line is also appended to this file. Useful to "
                       "record an episode in the OPOSSUM log format. Empty = disabled.");
        p.skillFilter = opossum::SkillEventRenderer::DefaultNoiseSkillsString();
        defs->optional(p.skillFilter,
                       "opossum.SkillFilter",
                       "Comma-separated skills that are not reported. These are pure body and "
                       "gaze control primitives, which say nothing at scene level.");
        defs->optional(p.resolveObjectNames,
                       "opossum.ResolveObjectNames",
                       "Resolve object classes to their natural-language names using "
                       "PriorKnowledgeData. If false, the class name is humanized instead.");
 
        defs->optional(p.memoryName, "mem.SkillMemoryName", "Name of the skill memory.");
        defs->optional(p.coreSegmentName,
                       "mem.CoreSegmentName",
                       "Name of the core segment holding the skill events.");
 
        return defs;
    }
 
    void
    OpossumLogger::onInitComponent()
    {
        skillEventID = armem::MemoryID(p.memoryName, p.coreSegmentName);
 
        opossum::SpokenNameLookup lookup;
        if (p.resolveObjectNames)
        {
            lookup = spokenNames.asLookup();
        }
        renderer = std::make_unique<opossum::SkillEventRenderer>(
            opossum::SkillEventRenderer::ParseSkillList(p.skillFilter), std::move(lookup));
 
        if (not p.enabled)
        {
            ARMARX_IMPORTANT << "Reporting is disabled (opossum.Enabled = false).";
        }
    }
 
    void
    OpossumLogger::onConnectComponent()
    {
        if (not p.enabled)
        {
            return;
        }
 
        // Wait for the memory to become available and add it as a dependency.
        ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
        try
        {
            skillMemoryReader = memoryNameSystem().useReader(p.memoryName);
        }
        catch (const armem::error::ArMemError& e)
        {
            ARMARX_ERROR << "Could not use the memory '" << p.memoryName
                         << "'. No skill events will be reported. Reason: " << e.what();
            return;
        }
 
        startSender();
 
        subscription =
            memoryNameSystem().subscribe(skillEventID, this, &OpossumLogger::onSkillEventUpdate);
 
        // `MemoryID` already prints itself in quotes.
        ARMARX_IMPORTANT << "Reporting skill events from " << skillEventID << " as '" << p.robotName
                         << "' to http://" << p.host << ":" << p.port << "/" << p.path << ".";
    }
 
    void
    OpossumLogger::onDisconnectComponent()
    {
        subscription.release();
        stopSender();
    }
 
    void
    OpossumLogger::onExitComponent()
    {
        stopSender();
    }
 
    bool
    OpossumLogger::isNew(const armem::MemoryID& snapshotID)
    {
        const std::string id = snapshotID.str();
 
        std::scoped_lock lock(seenMutex);
        if (not seen.insert(id).second)
        {
            return false;
        }
        seenOrder.push_back(id);
        if (seenOrder.size() > SeenCapacity)
        {
            seen.erase(seenOrder.front());
            seenOrder.pop_front();
        }
        return true;
    }
 
    std::map<armem::MemoryID, armem::wm::EntityInstance>
    OpossumLogger::resolve(const std::vector<armem::MemoryID>& snapshotIDs)
    {
        // Deliberately not memoryNameSystem().resolveEntityInstances(): that
        // re-resolves the MNS per call, ice_ping()ing every proxy of every
        // registered memory server, and this component runs off-robot.
        armem::client::QueryResult result = skillMemoryReader.queryMemoryIDs(snapshotIDs);
        if (not result.success)
        {
            ARMARX_WARNING << deactivateSpam(10) << "Could not query " << snapshotIDs.size()
                           << " skill event snapshot(s): " << result.errorMessage;
            return {};
        }
 
        std::map<armem::MemoryID, armem::wm::EntityInstance> instances;
        for (const armem::MemoryID& snapshotID : snapshotIDs)
        {
            try
            {
                instances.emplace(snapshotID, result.memory.getSnapshot(snapshotID).getInstance(0));
            }
            catch (const armem::error::ArMemError& e)
            {
                // The working memory holds a bounded history per entity, so a
                // snapshot can be evicted before we get to it.
                ARMARX_WARNING << deactivateSpam(10) << "Skill event " << snapshotID
                               << " is not in the memory (anymore): " << e.what();
            }
        }
        return instances;
    }
 
    void
    OpossumLogger::onSkillEventUpdate(const armem::MemoryID& /*subscriptionID*/,
                                      const std::vector<armem::MemoryID>& updatedSnapshotIDs)
    {
        std::vector<armem::MemoryID> newSnapshotIDs;
        for (const armem::MemoryID& snapshotID : updatedSnapshotIDs)
        {
            // The entity name is the skill name, so noise is dropped before it
            // costs a query -- and never displaces a real event from `seen`.
            if (renderer->isNoiseSkill(snapshotID.entityName))
            {
                continue;
            }
            // The same snapshot may be announced more than once; a duplicate
            // line would be a duplicate event to the summariser.
            if (isNew(snapshotID))
            {
                newSnapshotIDs.push_back(snapshotID);
            }
        }
        if (newSnapshotIDs.empty())
        {
            return;
        }
 
        // Resolving in one batch is a single query to the memory server.
        std::map<armem::MemoryID, armem::wm::EntityInstance> instances;
        try
        {
            instances = resolve(newSnapshotIDs);
        }
        catch (const std::exception& e)
        {
            ARMARX_WARNING << deactivateSpam(10) << "Could not read " << newSnapshotIDs.size()
                           << " skill event snapshot(s): " << e.what();
            return;
        }
 
        std::vector<std::pair<armarx::core::time::DateTime, std::string>> lines;
        for (const auto& [snapshotID, instance] : instances)
        {
            if (instance.data() == nullptr)
            {
                continue;
            }
 
            try
            {
                const auto update =
                    skills::arondto::SkillStatusUpdate::FromAron(instance.data());
 
                opossum::SkillEvent event;
                event.skillName = update.skillId.skillName;
                event.status = update.status;
                event.executorName = update.executorName;
                // The snapshot's referenced time is the time of the event;
                // executionStartedTimestamp is constant over an execution's
                // lifecycle events and would collapse them onto one instant.
                event.timestamp = snapshotID.timestamp;
 
                namespace converter = armarx::aron::data::converter;
                if (update.parameters)
                {
                    event.parameters =
                        converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(
                            update.parameters);
                }
                if (update.result)
                {
                    event.result = converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(
                        update.result);
                }
 
                if (const std::optional<std::string> line = renderer->render(event))
                {
                    lines.emplace_back(event.timestamp, *line);
                }
            }
            catch (const std::exception& e)
            {
                ARMARX_WARNING << deactivateSpam(10) << "Could not render the skill event "
                               << snapshotID << ": " << e.what();
            }
        }
 
        // A batch may contain events of different skills; report them in the
        // order in which they happened.
        std::sort(lines.begin(),
                  lines.end(),
                  [](const auto& lhs, const auto& rhs)
                  { return lhs.first.toMicroSecondsSinceEpoch() < rhs.first.toMicroSecondsSinceEpoch(); });
        for (auto& [time, line] : lines)
        {
            enqueue(std::move(line));
        }
    }
 
    void
    OpossumLogger::enqueue(std::string line)
    {
        ARMARX_VERBOSE << line;
 
        std::size_t dropped = 0;
        {
            std::scoped_lock lock(queueMutex);
            queue.push_back(std::move(line));
            while (queue.size() > static_cast<std::size_t>(std::max(1, p.queueSize)))
            {
                queue.pop_front();
                dropped = ++droppedLines;
            }
        }
        queueCondition.notify_one();
 
        if (dropped > 0)
        {
            ARMARX_WARNING << deactivateSpam(10) << "Dropped the oldest of more than "
                           << p.queueSize << " pending log lines (" << dropped
                           << " in total). The OPOSSUM server is not keeping up.";
        }
    }
 
    void
    OpossumLogger::startSender()
    {
        if (running.exchange(true))
        {
            return;
        }
        sender = std::thread(&OpossumLogger::sendLoop, this);
    }
 
    void
    OpossumLogger::stopSender()
    {
        if (not running.exchange(false))
        {
            return;
        }
        queueCondition.notify_all();
        if (sender.joinable())
        {
            sender.join();
        }
    }
 
    void
    OpossumLogger::sendLoop()
    {
        // cpp-httplib ignores SIGPIPE only in its Server constructor, never in
        // Client, and does not pass MSG_NOSIGNAL; ArmarX installs no SIGPIPE
        // handler either. Writing to a socket the summary server has already
        // closed would therefore take down the whole robot process. Blocked for
        // this thread only rather than process-wide: send() then fails with
        // EPIPE, which httplib reports as a failed Result, handled below.
        sigset_t blocked;
        sigemptyset(&blocked);
        sigaddset(&blocked, SIGPIPE);
        pthread_sigmask(SIG_BLOCK, &blocked, nullptr);
 
        httplib::Client client(p.host, p.port);
        client.set_connection_timeout(std::chrono::milliseconds(p.timeoutMs));
        client.set_read_timeout(std::chrono::milliseconds(p.timeoutMs));
        client.set_write_timeout(std::chrono::milliseconds(p.timeoutMs));
 
        std::ofstream file;
        if (not p.outputFile.empty())
        {
            file.open(p.outputFile, std::ios::out | std::ios::app);
            if (not file.is_open())
            {
                ARMARX_WARNING << "Could not open '" << p.outputFile
                               << "' for writing. Log lines will only be posted.";
            }
        }
 
        const std::string path = "/" + p.path;
        bool reachable = true;
        // Set once the component starts shutting down; until then, pending
        // lines are still sent, but not forever.
        std::optional<std::chrono::steady_clock::time_point> drainUntil;
 
        while (true)
        {
            std::string line;
            {
                std::unique_lock lock(queueMutex);
                queueCondition.wait(lock,
                                    [this] { return not running.load() or not queue.empty(); });
                if (queue.empty())
                {
                    // Shutting down and nothing left to send.
                    break;
                }
                if (not running.load())
                {
                    if (not drainUntil.has_value())
                    {
                        drainUntil = std::chrono::steady_clock::now() + DrainTimeout;
                    }
                    if (std::chrono::steady_clock::now() > *drainUntil)
                    {
                        ARMARX_INFO << "Discarding " << queue.size()
                                    << " pending log line(s) on shutdown.";
                        break;
                    }
                }
                line = std::move(queue.front());
                queue.pop_front();
            }
 
            if (file.is_open())
            {
                file << line << "\n";
                file.flush();
            }
 
            const nlohmann::json message{{"robot_name", p.robotName}, {"message", line}};
            const std::string payload = message.dump();
 
            // Only at verbose level: the full request, so what reaches the
            // server can be checked without capturing the traffic.
            ARMARX_VERBOSE << "Publishing to http://" << p.host << ":" << p.port << path
                           << " (" << payload.size() << " bytes): " << payload;
 
            bool posted = false;
            try
            {
                // The server may be turned off; that is expected and must not
                // affect the robot in any way.
                const httplib::Result result = client.Post(path, payload, "application/json");
                // `result` holds no response when the request never completed,
                // so it must not be dereferenced then. That case means the
                // server is unreachable, reported by the tracking below.
                posted = static_cast<bool>(result);
                if (posted and (result->status < 200 or result->status >= 300))
                {
                    // The server answered, so it is reachable, but it did not
                    // accept the line -- a wrong opossum.Path, or an error on
                    // its side. Worth its own warning: the summary server is
                    // run by a project partner, we do not see its console, and
                    // every rejected line is lost.
                    ARMARX_WARNING << deactivateSpam(60) << "The OPOSSUM server at http://"
                                   << p.host << ":" << p.port << path << " answered "
                                   << result->status << " " << result->reason
                                   << ". The log line was discarded. If this is a 404, check "
                                   << "opossum.Path against the path the server serves.";
                }
                else if (posted)
                {
                    ARMARX_VERBOSE << "Published (" << result->status << " " << result->reason
                                   << "), answer: " << result->body;
                }
            }
            catch (const std::exception& e)
            {
                ARMARX_WARNING << deactivateSpam(60) << "Failed to post a log line to http://"
                               << p.host << ":" << p.port << path << ": " << e.what();
            }
 
            if (posted != reachable)
            {
                reachable = posted;
                if (reachable)
                {
                    ARMARX_IMPORTANT << "The OPOSSUM server at http://" << p.host << ":" << p.port
                                     << path << " is reachable again.";
                }
                else
                {
                    ARMARX_WARNING << "The OPOSSUM server at http://" << p.host << ":" << p.port
                                   << path << " is not reachable. Log lines are discarded until "
                                   << "it comes back. This is not an error on the robot's side.";
                }
            }
        }
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(OpossumLogger, OpossumLogger::GetDefaultName());
 
} // namespace armarx